srenblad/nalgebra
1
0
Fork 0
forked from M-Labs/nalgebra
Code Pull Requests 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) 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 /// Algorithm from: Oshman, Yaakov, and Avishy Carmi. "Attitude estimation from vector
/// observations using a genetic-algorithm-embedded quaternion particle filter." Journal of /// 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 q3 = UnitQuaternion::from_euler_angles(0.1, 0.0, 0.0);
/// ///
/// let quat_vec = vec![q1, q2, q3]; /// 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(); /// let euler_angles_mean = q_mean.euler_angles();
/// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7) /// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7)
/// ``` /// ```
#[inline] #[inline]
pub fn quaternions_mean(unit_quaternions: &Vec<Self>) -> Self { pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self {
assert!(unit_quaternions.len() > 0);
let quaternions_matrix: Matrix4<N> = unit_quaternions let quaternions_matrix: Matrix4<N> = unit_quaternions
.iter() .into_iter()
.map(|q| q.as_vector() * q.as_vector().transpose()) .map(|q| q.as_vector() * q.as_vector().transpose())
.sum(); .sum();
assert!(!quaternions_matrix.is_zero());
let eigen_matrix = quaternions_matrix let eigen_matrix = quaternions_matrix
.try_symmetric_eigen(N::RealField::default_epsilon(), 10) .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 let max_eigenvalue_index = eigen_matrix
.eigenvalues .eigenvalues