def test_from_matrix():
T_good = SE3.from_matrix(torch.eye(4))
assert isinstance(T_good, SE3) \
and isinstance(T_good.rot, SO3) \
and T_good.trans.shape == (3,) \
and SE3.is_valid_matrix(T_good.as_matrix()).all()
T_bad = SE3.from_matrix(torch.eye(4).add_(1e-3), normalize=True)
assert isinstance(T_bad, SE3) \
and isinstance(T_bad.rot, SO3) \
and T_bad.trans.shape == (3,) \
and SE3.is_valid_matrix(T_bad.as_matrix()).all()
def test_from_matrix_batch():
T_good = SE3.from_matrix(torch.eye(4).repeat(5, 1, 1))
assert isinstance(T_good, SE3) \
and T_good.trans.shape == (5, 3) \
and SE3.is_valid_matrix(T_good.as_matrix()).all()
T_bad = T_good.as_matrix()
T_bad[3, :, :].add_(0.1)
T_bad = SE3.from_matrix(T_bad, normalize=True)
assert isinstance(T_bad, SE3) \
and T_bad.trans.shape == (5, 3) \
and SE3.is_valid_matrix(T_bad.as_matrix()).all()
def compute_mate(self, t, x, chi, dataset_name):
chi_est = torch.zeros(x.shape[0], 4, 4)
chi_est[:, :3, :3] = SO3.from_rpy(x[:, 3:6]).as_matrix()
chi_est[:, :3, 3] = x[:, :3]
chi_est[:, 3, 3] = 1
chi_est = SE3.from_matrix(chi_est)
chi = SE3.from_matrix(chi)
error = (chi.inv().dot(chi_est)).log()
mate_translation = error[:, :3].abs().mean()
mate_rotation = error[:, 3:].abs().mean()
return mate_translation, mate_rotation
def calculate_log_se3_delta(predicted_position, target_position):
predicted_matrix = predicted_position.matrix
target_matrix = target_position.matrix
delta_matrix = torch.bmm(inverse_pose_matrix(predicted_matrix),
target_matrix)
delta_log = SE3.log(
SE3.from_matrix(delta_matrix, normalize=False, check=False))
if delta_log.dim() < 2:
delta_log = delta_log[None]
return delta_log
def log_prob(self, value_matrix, mean_matrix, logvar):
if logvar.dim() < 2:
logvar = logvar[None].expand(mean_matrix.shape[0], logvar.shape[0])
delta_matrix = torch.bmm(inverse_pose_matrix(mean_matrix), value_matrix)
delta_log = SE3.log(SE3.from_matrix(delta_matrix, normalize=False, check=False))
if delta_log.dim() < 2:
delta_log = delta_log[None]
inverse_sigma_matrix = self.get_inverse_sigma_matrix(logvar).expand(delta_log.shape[0], delta_log.shape[1],
delta_log.shape[1])
delta_log = torch.bmm(inverse_sigma_matrix, delta_log[:, :, None])[:, :, 0]
log_determinant = self.get_logvar_determinant(logvar)
log_prob = torch.sum(delta_log ** 2 / 2., dim=1) + 0.5 * log_determinant
return torch.mean(log_prob)
def mean_position(self, predicted_position):
batch_size = predicted_position.shape[0]
predicted_position = predicted_position.reshape(batch_size * self._head_count,
predicted_position.shape[1] // self._head_count)
logvar = predicted_position[:, 7:]
mean_matrix = self.mean_matrix(predicted_position)
log_mean = SE3.log(SE3.from_matrix(mean_matrix, normalize=False, check=False))
if log_mean.dim() < 2:
log_mean = log_mean[None]
inverse_sigma_matrix = self.get_inverse_sigma_matrix(logvar)
inverse_covariance_matrix = torch.bmm(inverse_sigma_matrix.transpose(1, 2), inverse_sigma_matrix)
result_inverse_covariance_matrix = torch.sum(inverse_covariance_matrix.reshape(-1, self._head_count, 6, 6),
dim=1)
result_covariance_matrix = torch.inverse(result_inverse_covariance_matrix)
factors = torch.bmm(result_covariance_matrix.repeat_interleave(self._head_count, 0), inverse_covariance_matrix)
scaled_log_mean = torch.bmm(factors, log_mean[:, :, None])[:, :, 0]
result_log_mean = torch.sum(scaled_log_mean.reshape(-1, self._head_count, 6), dim=1)
mean_matrix = SE3.exp(result_log_mean).as_matrix()
if mean_matrix.dim() < 3:
mean_matrix = mean_matrix[None]
return mean_matrix
def test_dot():
T = torch.Tensor([[0, 0, -1, 0.1],
[0, 1, 0, 0.5],
[1, 0, 0, -0.5],
[0, 0, 0, 1]])
T_SE3 = SE3.from_matrix(T)
pt = torch.Tensor([1, 2, 3])
pth = torch.Tensor([1, 2, 3, 1])
TT = torch.mm(T, T)
TT_SE3 = T_SE3.dot(T_SE3).as_matrix()
assert utils.allclose(TT_SE3, TT)
Tpt = torch.matmul(T[0:3, 0:3], pt) + T[0:3, 3]
Tpt_SE3 = T_SE3.dot(pt)
assert utils.allclose(Tpt_SE3, Tpt)
Tpth = torch.matmul(T, pth)
Tpth_SE3 = T_SE3.dot(pth)
assert utils.allclose(Tpth_SE3, Tpth) and \
utils.allclose(Tpth_SE3[0:3], Tpt)
def box_minus(self, chi_1, chi_2):
return SE3.from_matrix(chi_2).inv().dot(SE3.from_matrix(chi_1)).log()
def test_dot_batch():
T1 = torch.Tensor([[0, 0, -1, 0.1],
[0, 1, 0, 0.5],
[1, 0, 0, -0.5],
[0, 0, 0, 1]]).expand(5, 4, 4)
T2 = torch.Tensor([[0, 0, -1, 0.1],
[0, 1, 0, 0.5],
[1, 0, 0, -0.5],
[0, 0, 0, 1]])
T1_SE3 = SE3.from_matrix(T1)
T2_SE3 = SE3.from_matrix(T2)
pt1 = torch.Tensor([1, 2, 3])
pt2 = torch.Tensor([4, 5, 6])
pt3 = torch.Tensor([7, 8, 9])
pts = torch.cat([pt1.unsqueeze(dim=0),
pt2.unsqueeze(dim=0),
pt3.unsqueeze(dim=0)], dim=0) # 3x3
ptsbatch = pts.unsqueeze(dim=0).expand(5, 3, 3)
pt1h = torch.Tensor([1, 2, 3, 1])
pt2h = torch.Tensor([4, 5, 6, 1])
pt3h = torch.Tensor([7, 8, 9, 1])
ptsh = torch.cat([pt1h.unsqueeze(dim=0),
pt2h.unsqueeze(dim=0),
pt3h.unsqueeze(dim=0)], dim=0) # 3x4
ptshbatch = ptsh.unsqueeze(dim=0).expand(5, 3, 4)
T1T1 = torch.bmm(T1, T1)
T1T1_SE3 = T1_SE3.dot(T1_SE3).as_matrix()
assert T1T1_SE3.shape == T1.shape and utils.allclose(T1T1_SE3, T1T1)
T1T2 = torch.matmul(T1, T2)
T1T2_SE3 = T1_SE3.dot(T2_SE3).as_matrix()
assert T1T2_SE3.shape == T1.shape and utils.allclose(T1T2_SE3, T1T2)
T1pt1 = torch.matmul(T1[:, 0:3, 0:3], pt1) + T1[:, 0:3, 3]
T1pt1_SE3 = T1_SE3.dot(pt1)
assert T1pt1_SE3.shape == (T1.shape[0], pt1.shape[0]) \
and utils.allclose(T1pt1_SE3, T1pt1)
T1pt1h = torch.matmul(T1, pt1h)
T1pt1h_SE3 = T1_SE3.dot(pt1h)
assert T1pt1h_SE3.shape == (T1.shape[0], pt1h.shape[0]) \
and utils.allclose(T1pt1h_SE3, T1pt1h) \
and utils.allclose(T1pt1h_SE3[:, 0:3], T1pt1_SE3)
T1pt2 = torch.matmul(T1[:, 0:3, 0:3], pt2) + T1[:, 0:3, 3]
T1pt2_SE3 = T1_SE3.dot(pt2)
assert T1pt2_SE3.shape == (T1.shape[0], pt2.shape[0]) \
and utils.allclose(T1pt2_SE3, T1pt2)
T1pt2h = torch.matmul(T1, pt2h)
T1pt2h_SE3 = T1_SE3.dot(pt2h)
assert T1pt2h_SE3.shape == (T1.shape[0], pt2h.shape[0]) \
and utils.allclose(T1pt2h_SE3, T1pt2h) \
and utils.allclose(T1pt2h_SE3[:, 0:3], T1pt2_SE3)
T1pts = torch.bmm(T1[:, 0:3, 0:3],
pts.unsqueeze(dim=0).expand(
T1.shape[0],
pts.shape[0],
pts.shape[1]).transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:3, 3].unsqueeze(dim=1).expand(
T1.shape[0], pts.shape[0], pts.shape[1])
T1pts_SE3 = T1_SE3.dot(pts)
assert T1pts_SE3.shape == (T1.shape[0], pts.shape[0], pts.shape[1]) \
and utils.allclose(T1pts_SE3, T1pts) \
and utils.allclose(T1pt1, T1pts[:, 0, :]) \
and utils.allclose(T1pt2, T1pts[:, 1, :])
T1ptsh = torch.bmm(T1, ptsh.unsqueeze(dim=0).expand(
T1.shape[0],
ptsh.shape[0],
ptsh.shape[1]).transpose(2, 1)).transpose(2, 1)
T1ptsh_SE3 = T1_SE3.dot(ptsh)
assert T1ptsh_SE3.shape == (T1.shape[0], ptsh.shape[0], ptsh.shape[1]) \
and utils.allclose(T1ptsh_SE3, T1ptsh) \
and utils.allclose(T1pt1h, T1ptsh[:, 0, :]) \
and utils.allclose(T1pt2h, T1ptsh[:, 1, :]) \
and utils.allclose(T1ptsh_SE3[:, :, 0:3], T1pts_SE3)
T1ptsbatch = torch.bmm(T1[:, 0:3, 0:3],
ptsbatch.transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:3, 3].unsqueeze(dim=1).expand(ptsbatch.shape)
T1ptsbatch_SE3 = T1_SE3.dot(ptsbatch)
assert T1ptsbatch_SE3.shape == ptsbatch.shape \
and utils.allclose(T1ptsbatch_SE3, T1ptsbatch) \
and utils.allclose(T1pt1, T1ptsbatch[:, 0, :]) \
and utils.allclose(T1pt2, T1ptsbatch[:, 1, :])
T1ptshbatch = torch.bmm(T1, ptshbatch.transpose(2, 1)).transpose(2, 1)
T1ptshbatch_SE3 = T1_SE3.dot(ptshbatch)
assert T1ptshbatch_SE3.shape == ptshbatch.shape \
and utils.allclose(T1ptshbatch_SE3, T1ptshbatch) \
and utils.allclose(T1pt1h, T1ptshbatch[:, 0, :]) \
and utils.allclose(T1pt2h, T1ptshbatch[:, 1, :]) \
and utils.allclose(T1ptshbatch_SE3[:, :, 0:3], T1ptsbatch_SE3)
T2ptsbatch = torch.matmul(T2[0:3, 0:3],
ptsbatch.transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:3, 3].unsqueeze(dim=1).expand(ptsbatch.shape)
T2ptsbatch_SE3 = T2_SE3.dot(ptsbatch)
assert T2ptsbatch_SE3.shape == ptsbatch.shape \
and utils.allclose(T2ptsbatch_SE3, T2ptsbatch) \
and utils.allclose(T2_SE3.dot(pt1), T2ptsbatch[:, 0, :]) \
and utils.allclose(T2_SE3.dot(pt2), T2ptsbatch[:, 1, :])
T2ptshbatch = torch.matmul(T2, ptshbatch.transpose(2, 1)).transpose(2, 1)
T2ptshbatch_SE3 = T2_SE3.dot(ptshbatch)
assert T2ptshbatch_SE3.shape == ptshbatch.shape \
and utils.allclose(T2ptshbatch_SE3, T2ptshbatch) \
and utils.allclose(T2_SE3.dot(pt1h), T2ptshbatch[:, 0, :]) \
and utils.allclose(T2_SE3.dot(pt2h), T2ptshbatch[:, 1, :]) \
and utils.allclose(T2ptshbatch_SE3[:, :, 0:3], T2ptsbatch_SE3)