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

Perturbations to check for convergence into maximum.

This commit is contained in:
Tim Taubner 2022-04-01 14:30:20 +02:00 committed by Sébastien Crozet
parent ac203fe4fd
commit d515e4f1be
1 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/geometry/rotation_specialization.rs
View File

@ -17,7 +17,7 @@ use std::ops::Neg;
use crate::base::dimension::{U1, U2, U3};
use crate::base::storage::Storage;
use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3, UnitVector3};
use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
@ -706,15 +706,12 @@ 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 + crate::Scalar,
Standard: Distribution<Rotation3<T>>,
T: RealField,
{
// Starting from a random rotation has almost zero likelihood to end up in a maximum if `m` is already a rotation matrix
let random_rotation: Rotation3<T> = rand::thread_rng().gen();
Self::from_matrix_eps(m, T::default_epsilon(), 0, random_rotation)
Self::from_matrix_eps(m, T::default_epsilon(), 0, Rotation3::identity())
}
/// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
@ -737,6 +734,7 @@ where
max_iter = usize::MAX;
}
let mut perturbation_axes = UnitVector3::new_unchecked(Vector3::new(T::one(), T::zero(), T::zero()));
let mut rot = guess.into_inner();
for _ in 0..max_iter {
@ -752,9 +750,27 @@ where
if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps.clone()) {
rot = Rotation3::from_axis_angle(&axis, angle) * rot;
} else {
// Check if stuck in a maximum w.r.t. the norm (m - rot).norm()
let mut perturbed = rot.clone();
let norm_squared = (m - &rot).norm_squared();
let mut new_norm_squared: T;
// Perturb until the new norm is significantly different
loop {
perturbed *= Rotation3::from_axis_angle(&perturbation_axes, T::frac_pi_8());
new_norm_squared = (m - &perturbed).norm_squared();
if relative_ne!(norm_squared, new_norm_squared) {
break;
}
}
// If new norm is larger, it's a minimum
if norm_squared < new_norm_squared {
break;
}
// If not, continue from perturbed rotation, but use a different axes for the next perturbation
perturbation_axes = UnitVector3::new_unchecked(Vector3::new(perturbation_axes.y.clone(), perturbation_axes.z.clone(), perturbation_axes.x.clone()));
rot = perturbed;
}
}
Self::from_matrix_unchecked(rot)
}