def generate_random_rotation_matrix(self): thetas = torch.zeros(3, dtype=torch.float) for axis_ind, deg_angle in enumerate(self._degree_angles): if deg_angle > 0: rand_deg_angle = random.random() * deg_angle rand_radian_angle = float(rand_deg_angle * np.pi) / 180.0 thetas[axis_ind] = rand_radian_angle return euler_angles_to_rotation_matrix(thetas, random_order=True)
def generate_random_rotation_matrix(self): thetas = [] for axis_ind, deg_angle in enumerate(self._degree_angles): if deg_angle > 0: rand_deg_angle = random.random() * deg_angle rand_radian_angle = float(rand_deg_angle * np.pi) / 180.0 thetas.append(torch.tensor(rand_radian_angle)) else: thetas.append(torch.tensor(0.0)) return euler_angles_to_rotation_matrix(thetas)
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_ransac(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 = ransac_registration(a, b, distance_threshold=0.01) # T_pred = estimate_transfo(a, b) npt.assert_allclose(T_pred.numpy(), T_gt.numpy(), rtol=1e-3)
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)
def test_compute_scaled_registration_error(self): xyz = torch.tensor([ [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0], [0.0, 2.0, 0.0], [2.0, 0.0, 0.0], ]) R_est = euler_angles_to_rotation_matrix(torch.tensor([0, 0, np.pi / 6])) T_gt = torch.eye(4) T_est = torch.eye(4) T_est[:3, :3] = R_est err = compute_scaled_registration_error(xyz, T_gt, T_est) val = 0.55901 self.assertAlmostEqual(err.item(), val, places=5)