Clean up implementation

pytransform3d/test/test_uncertainty.py

@@ -7,14 +7,14 @@
 
 
 def test_estimate_gaussian_rotation_matrix_from_samples():
- mean = pr.matrix_from_axis_angle([0.3, 0.2, 0.5, 0.25 * np.pi])
- cov = np.diag([1.3, 1.5, 2.9])
  rng = np.random.default_rng(34)
+ mean = pr.matrix_from_axis_angle([0.3, 0.2, 0.5, 0.25 * np.pi])
+ cov = np.diag([0.1, 0.05, 0.09]) ** 2
  samples = np.array([pr.random_matrix(rng, mean, cov) for _ in range(1000)])
  mean_est, cov_est = pu.estimate_gaussian_rotation_matrix_from_samples(
  samples)
- assert_array_almost_equal(mean, mean_est, decimal=1)
- assert_array_almost_equal(cov, cov_est, decimal=0)
+ assert_array_almost_equal(mean, mean_est, decimal=2)
+ assert_array_almost_equal(cov, cov_est, decimal=2)
 
 
 def test_same_fuse_poses():

pytransform3d/uncertainty.py

@@ -1,7 +1,7 @@
 """Operations related to uncertain transformations."""
 import numpy as np
-from .batch_rotations import (matrices_from_compact_axis_angles,
- axis_angles_from_matrices)
+from .rotations import matrix_from_compact_axis_angle, norm_matrix
+from .batch_rotations import axis_angles_from_matrices
 from .transformations import (
  invert_transform, transform_from, concat, adjoint_from_transform,
  left_jacobian_SE3_inv, transform_from_exponential_coordinates,
@@ -37,21 +37,14 @@ def estimate_gaussian_rotation_matrix_from_samples(samples):
  https://ethaneade.com/lie.pdf
  """
  assert len(samples) > 0
- samples = np.asarray(samples)
+ samples = np.asarray([norm_matrix(R) for R in samples])
  mean = samples[0]
  for _ in range(20):
  mean_inv = mean.T
- mean_diffs_axis_angle = axis_angles_from_matrices(
- # Uses implementation detail concat_many_to_one that is not part of
- # the public interface. It is supposed to support only homgeneous
- # transformation matrices, but it works for rotation matrices, too!
- concat_many_to_one(samples, mean_inv)
- )
- mean_diffs = (mean_diffs_axis_angle[:, :3]
- * mean_diffs_axis_angle[:, 3, np.newaxis])
+ mean_diffs = axis_angles_from_matrices(concat_many_to_one(samples, mean_inv))
+ mean_diffs = mean_diffs[:, :3] * mean_diffs[:, 3, np.newaxis]
  avg_mean_diff = np.mean(mean_diffs, axis=0)
- print(f"{np.linalg.norm(avg_mean_diff)=}")
- mean = np.dot(matrices_from_compact_axis_angles(avg_mean_diff), mean)
+ mean = np.dot(matrix_from_compact_axis_angle(avg_mean_diff), mean)
 
  cov = np.cov(mean_diffs, rowvar=False, bias=True)
  return mean, cov