From dacd15e927fe4ef29ecea2b637fd6826aeb86ff4 Mon Sep 17 00:00:00 2001
From: thibault <thibault.barbie@free.fr>
Date: Tue, 3 Sep 2019 02:52:49 +0900
Subject: [PATCH] Added function to compute the mean quaternion from a vector
 of unit quaternions.

---
 src/geometry/quaternion_construction.rs | 50 ++++++++++++++++++++++++-
 1 file changed, 49 insertions(+), 1 deletion(-)

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<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> {