M-Labs
/
nalgebra
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

From_rotation_matrix: Use the larger of eps.sqrt() or eps*eps as disturbance. Add tests for eps > 1.0

This commit is contained in:
Tim Taubner 2022-07-27 11:31:34 +02:00
parent 18a8a30671
commit 7aadbcf21d
3 changed files with 34 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/geometry/quaternion_construction.rs
View File

@ -410,7 +410,6 @@ where
/// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
#[cfg(feature = "rand-no-std")]
pub fn from_matrix(m: &Matrix3<T>) -> Self
where
T: RealField,

7
src/geometry/rotation_specialization.rs
View File

@ -735,6 +735,8 @@ where
max_iter = usize::MAX;
}
// Using sqrt(eps) ensures we perturb with something larger than eps; clamp to eps to handle the case of eps > 1.0
let eps_disturbance = eps.clone().sqrt().max(eps.clone() * eps.clone());
let mut perturbation_axes = Vector3::x_axis();
let mut rot = guess.into_inner();
@ -758,10 +760,9 @@ where
// Perturb until the new norm is significantly different
loop {
perturbed *= Rotation3::from_axis_angle(&perturbation_axes, T::frac_pi_8());
perturbed *= Rotation3::from_axis_angle(&perturbation_axes, eps_disturbance.clone());
new_norm_squared = (m - &perturbed).norm_squared();
if relative_ne!(norm_squared, new_norm_squared) {
if abs_diff_ne!(norm_squared, new_norm_squared, epsilon = T::default_epsilon()) {
break;
}
}

40
tests/geometry/rotation.rs
View File

@ -32,31 +32,51 @@ fn from_rotation_matrix() {
&Rotation3::from_axis_angle(&UnitVector3::new_unchecked(Vector3::new(1.0, 0.0, 0.0)), PI),
epsilon = 0.001
);
// Test that issue 628 is fixed
let m_628 = nalgebra::Matrix3::<f64>::new(-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
// Test that issue 627 is fixed
let m_627 = Matrix3::<f64>::new(-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
assert_relative_ne!(
identity,
nalgebra::Rotation3::from_matrix(&m_628),
Rotation3::from_matrix(&m_627),
epsilon = 0.01
);
assert_relative_eq!(
nalgebra::Rotation3::from_matrix_unchecked(m_628.clone()),
nalgebra::Rotation3::from_matrix(&m_628),
Rotation3::from_matrix_unchecked(m_627.clone()),
Rotation3::from_matrix(&m_627),
epsilon = 0.001
);
// Test that issue 1078 is fixed
let m_1078 = nalgebra::Matrix3::<f64>::new(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0);
let m_1078 = Matrix3::<f64>::new(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0);
assert_relative_ne!(
identity,
nalgebra::Rotation3::from_matrix(&m_1078),
Rotation3::from_matrix(&m_1078),
epsilon = 0.01
);
assert_relative_eq!(
nalgebra::Rotation3::from_matrix_unchecked(m_1078.clone()),
nalgebra::Rotation3::from_matrix(&m_1078),
Rotation3::from_matrix_unchecked(m_1078.clone()),
Rotation3::from_matrix(&m_1078),
epsilon = 0.001
);
// Additional test cases for eps >= 1.0
assert_relative_ne!(
identity,
Rotation3::from_matrix_eps(&m_627, 1.2, 0, Rotation3::identity()),
epsilon = 0.6
);
assert_relative_eq!(
Rotation3::from_matrix_unchecked(m_627.clone()),
Rotation3::from_matrix_eps(&m_627, 1.2, 0, Rotation3::identity()),
epsilon = 0.6
);
assert_relative_ne!(
identity,
Rotation3::from_matrix_eps(&m_1078, 1.0, 0, Rotation3::identity()),
epsilon = 0.1
);
assert_relative_eq!(
Rotation3::from_matrix_unchecked(m_1078.clone()),
Rotation3::from_matrix_eps(&m_1078, 1.0, 0, Rotation3::identity()),
epsilon = 0.1
);
}
#[test]