def test_composition(self):
U_1 = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
U_2 = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
U_3 = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
p_1 = torch.rand(1000, 3) @ U_1.T @ torch.diag(torch.tensor([1, 0.7, 0.5])) @ U_1
p_2 = torch.rand(1000, 3) @ U_2.T @ torch.diag(torch.tensor([1, 0.9, 0.001])) @ U_2
p_3 = torch.rand(1000, 3) @ U_3.T @ torch.diag(torch.tensor([1, 0.0001, 0.000001])) @ U_3
data_1 = Data(pos=p_1)
data_2 = Data(pos=p_2)
data_3 = Data(pos=p_3)
compose_filter = FCompose([PlanarityFilter(0.5, is_leq=True), PlanarityFilter(0.1, is_leq=False)])
self.assertTrue(compose_filter(data_1).item())
self.assertFalse(compose_filter(data_2).item())
self.assertFalse(compose_filter(data_3).item())
def test_fast_global_registration(self):
a = torch.randn(100, 3)
R_gt = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
t_gt = torch.rand(3)
T_gt = torch.eye(4)
T_gt[:3, :3] = R_gt
T_gt[:3, 3] = t_gt
b = a.mm(R_gt.T) + t_gt
T_pred = fast_global_registration(a, b, mu_init=1, num_iter=20)
npt.assert_allclose(T_pred.numpy(), T_gt.numpy(), rtol=1e-3)
def test_fast_global_registration_with_outliers(self):
a = torch.randn(100, 3)
R_gt = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
t_gt = torch.rand(3)
T_gt = torch.eye(4)
T_gt[:3, :3] = R_gt
T_gt[:3, 3] = t_gt
b = a.mm(R_gt.T) + t_gt
b[[1, 5, 20, 32, 74, 17, 27, 77, 88, 89]] *= 42
T_pred = fast_global_registration(a, b, mu_init=1, num_iter=20)
# T_pred = estimate_transfo(a, b)
npt.assert_allclose(T_pred.numpy(), T_gt.numpy(), rtol=1e-3)
def test_planarity_filter(self):
# Plane with high planarity
U = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
plane = torch.rand(1000, 3) @ U @ torch.diag(
torch.tensor([1, 1, 0.001])) @ U
data1 = Data(pos=plane)
# random isotropic gaussian
data2 = Data(pos=torch.randn(100, 3))
plane_filter = PlanarityFilter(0.3)
self.assertTrue(plane_filter(data2).item())
self.assertFalse(plane_filter(data1).item())
def test_estimate_transfo(self):
a = torch.randn(100, 3)
R_gt = euler_angles_to_rotation_matrix(torch.rand(3) * np.pi)
t_gt = torch.rand(3)
T_gt = torch.eye(4)
T_gt[:3, :3] = R_gt
T_gt[:3, 3] = t_gt
b = a.mm(R_gt.T) + t_gt
T_pred = estimate_transfo(a, b)
npt.assert_allclose(T_pred.numpy(), T_gt.numpy(), rtol=1e-3)