Added function to compute the mean quaternion from a vector of unit quaternions.

This commit is contained in:
thibault 2019-09-03 02:52:49 +09:00 committed by Sébastien Crozet
parent b5f43c6efc
commit dacd15e927
1 changed files with 49 additions and 1 deletions

View File

@ -13,7 +13,7 @@ use alga::general::RealField;
use crate::base::dimension::U3; use crate::base::dimension::U3;
use crate::base::storage::Storage; 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}; use crate::geometry::{Quaternion, Rotation3, UnitQuaternion};
@ -676,6 +676,54 @@ impl<N: RealField> UnitQuaternion<N> {
where SB: Storage<N, U3> { where SB: Storage<N, U3> {
Self::new_eps(axisangle, eps) 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> { impl<N: RealField> One for UnitQuaternion<N> {