Added function to compute the mean quaternion from a vector of unit quaternions.
@@ -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<N: RealField> UnitQuaternion<N> {
where SB: Storage<N, U3> {
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>) -> Self {
+ assert!(unit_quaternions.len() > 0);
+ let quaternions_matrix: Matrix4<N> = 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<N: RealField> One for UnitQuaternion<N> {
