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

Changed name. Changed argument. Added documentation line.

Browse Source
This commit is contained in:
thibault 2019-09-04 11:40:38 +09:00 committed by Sébastien Crozet
parent f475a9bc07
commit 8d89b02b70
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/geometry/quaternion_construction.rs
View File

@ -677,7 +677,10 @@ impl<N: RealField> UnitQuaternion<N> {
Self::new_eps(axisangle, eps)
}
/// Create the mean unit quaternion from a vector of unit quaternions.
/// Create the mean unit quaternion from a data structure implementing IntoIterator
/// returning unit quaternions.
///
/// The method will panic if the iterator does not return any quaternions.
///
/// Algorithm from: Oshman, Yaakov, and Avishy Carmi. "Attitude estimation from vector
/// observations using a genetic-algorithm-embedded quaternion particle filter." Journal of
@ -693,22 +696,23 @@ impl<N: RealField> UnitQuaternion<N> {
/// let q3 = UnitQuaternion::from_euler_angles(0.1, 0.0, 0.0);
///
/// let quat_vec = vec![q1, q2, q3];
/// let q_mean = UnitQuaternion::quaternions_mean(&quat_vec);
/// let q_mean = UnitQuaternion::mean_of(zero_vec);
///
/// let euler_angles_mean = q_mean.euler_angles();
/// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7)
/// ```
#[inline]
pub fn quaternions_mean(unit_quaternions: &Vec<Self>) -> Self {
assert!(unit_quaternions.len() > 0);
pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self {
let quaternions_matrix: Matrix4<N> = unit_quaternions
.iter()
.into_iter()
.map(|q| q.as_vector() * q.as_vector().transpose())
.sum();
assert!(!quaternions_matrix.is_zero());
let eigen_matrix = quaternions_matrix
.try_symmetric_eigen(N::RealField::default_epsilon(), 10)
.expect("Could not perform eigen decomposition when averaging quaternions.");
.expect("Quaternions matrix could not be diagonalized. This behavior should not be possible.");
let max_eigenvalue_index = eigen_matrix
.eigenvalues