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)
