diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index 893fdb6a..11f07d0e 100644 --- a/src/geometry/quaternion_construction.rs +++ b/src/geometry/quaternion_construction.rs @@ -13,7 +13,7 @@ use alga::general::RealField; use crate::base::dimension::U3; use crate::base::storage::Storage; -use crate::base::{Unit, Vector, Vector3, Vector4, Matrix3}; +use crate::base::{Unit, Vector, Vector3, Vector4, Matrix3, Matrix4}; use crate::geometry::{Quaternion, Rotation3, UnitQuaternion}; @@ -676,6 +676,54 @@ impl UnitQuaternion { where SB: Storage { Self::new_eps(axisangle, eps) } + + /// Create the mean unit quaternion from a vector of unit quaternions. + /// + /// Algorithm from: Oshman, Yaakov, and Avishy Carmi. "Attitude estimation from vector + /// observations using a genetic-algorithm-embedded quaternion particle filter." Journal of + /// Guidance, Control, and Dynamics 29.4 (2006): 879-891. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use std::f32; + /// # use nalgebra::{UnitQuaternion}; + /// let q1 = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0); + /// let q2 = UnitQuaternion::from_euler_angles(-0.1, 0.0, 0.0); + /// 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 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 { + assert!(unit_quaternions.len() > 0); + let quaternions_matrix: Matrix4 = unit_quaternions + .iter() + .map(|q| q.as_vector() * q.as_vector().transpose()) + .sum(); + + let eigen_matrix = quaternions_matrix + .try_symmetric_eigen(N::RealField::default_epsilon(), 10) + .expect("Could not perform eigen decomposition when averaging quaternions."); + + let max_eigenvalue_index = eigen_matrix + .eigenvalues + .iter() + .position(|v| *v == eigen_matrix.eigenvalues.max()) + .unwrap(); + + let max_eigenvector = eigen_matrix.eigenvectors.column(max_eigenvalue_index); + UnitQuaternion::from_quaternion(Quaternion::new( + max_eigenvector[0], + max_eigenvector[1], + max_eigenvector[2], + max_eigenvector[3], + )) + } } impl One for UnitQuaternion {