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)