quaternion_averager/
lib.rs

1use nalgebra::{
2    geometry::Quaternion, geometry::UnitQuaternion, linalg::SymmetricEigen, Matrix4, RealField,
3};
4
5/// A quaternion averager.
6/// Implemented as discussed [here](https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions)
7pub struct QuaternionAverager<T: RealField + Copy + PartialEq> {
8    matrix: Matrix4<T>,
9    weight_sum: T,
10}
11
12impl<T: RealField + Copy + PartialEq> Default for QuaternionAverager<T> {
13    fn default() -> Self {
14        Self::new()
15    }
16}
17
18impl<T: RealField + Copy + PartialEq> QuaternionAverager<T> {
19    /// Creates and returns a new quaternion averager
20    ///
21    /// # Example
22    ///
23    /// ```
24    /// use quaternion_averager::QuaternionAverager;
25    ///
26    /// let mut qa = QuaternionAverager::<f64>::new();
27    /// ```
28    pub fn new() -> QuaternionAverager<T> {
29        QuaternionAverager {
30            matrix: Matrix4::zeros(),
31            weight_sum: T::from_f32(0f32).unwrap(),
32        }
33    }
34
35    /// Add a new unit quaternion with weight 1 to the averager
36    ///
37    /// # Example
38    /// ```
39    /// use quaternion_averager::QuaternionAverager;
40    /// use nalgebra::{
41    ///     geometry::Quaternion,
42    ///     geometry::UnitQuaternion,
43    /// };
44    ///
45    /// let mut qa = QuaternionAverager::new();
46    /// let q1 = Quaternion::new(0.9961947f32, 0.0871557f32, 0f32, 0f32);
47    /// let q1 = UnitQuaternion::from_quaternion(q1);
48    /// qa.add_quaternion(&q1);
49    /// ```
50    pub fn add_quaternion(&mut self, quaternion: &UnitQuaternion<T>) {
51        let q = quaternion.coords * quaternion.coords.transpose();
52        self.matrix += q;
53        let w = T::from_f32(1f32).unwrap();
54        self.weight_sum += w;
55    }
56
57    /// Add a new unit quaternion with custom weight to the averager
58    ///
59    /// # Example
60    /// ```
61    /// use quaternion_averager::QuaternionAverager;
62    /// use nalgebra::{
63    ///     geometry::Quaternion,
64    ///     geometry::UnitQuaternion,
65    /// };
66    ///
67    /// let mut qa = QuaternionAverager::new();
68    /// let q1 = Quaternion::new(0.9961947f32, 0.0871557f32, 0f32, 0f32);
69    /// let q1 = UnitQuaternion::from_quaternion(q1);
70    /// qa.add_quaternion_weighted(&q1, 0.25f32);
71    /// ```
72    pub fn add_quaternion_weighted(&mut self, quaternion: &UnitQuaternion<T>, weight: T) {
73        let q = quaternion.coords * quaternion.coords.transpose();
74        let q = q / weight;
75        self.matrix += q;
76        self.weight_sum += weight;
77    }
78
79    /// Calculates and returns the quaternion average
80    ///
81    /// # Example
82    /// ```
83    /// use quaternion_averager::QuaternionAverager;
84    /// use nalgebra::{
85    ///     geometry::Quaternion,
86    ///     geometry::UnitQuaternion,
87    /// };
88    ///
89    /// let mut qa = QuaternionAverager::new();
90    /// let q1 = Quaternion::new(0.9961947f32, 0.0871557f32, 0f32, 0f32);
91    /// let q1 = UnitQuaternion::from_quaternion(q1);
92    /// let q2 = Quaternion::new(0.9848078f32, 0.1736482f32, 0f32, 0f32);
93    /// let q2 = UnitQuaternion::from_quaternion(q2);
94    /// qa.add_quaternion(&q1);
95    /// qa.add_quaternion(&q2);
96    /// let qavg = qa.calc_average();
97    /// println!("The average of {} and {} is {}", q1, q2, qavg);
98    /// ```
99    pub fn calc_average(&self) -> UnitQuaternion<T> {
100        let m = self.matrix / self.weight_sum;
101        let decomp = SymmetricEigen::new(m);
102        let i = decomp.eigenvalues.imax();
103        let q = decomp.eigenvectors.column(i);
104        let q = Quaternion::new(q[3], q[0], q[1], q[2]);
105
106        UnitQuaternion::from_quaternion(q)
107    }
108}
109
110#[cfg(test)]
111mod tests {
112    use super::*;
113    use approx::*;
114
115    #[test]
116    fn avg2_32() {
117        let mut avg = QuaternionAverager::new();
118        let q1 = Quaternion::new(0.9961947f32, 0.0871557f32, 0f32, 0f32);
119        let q1 = UnitQuaternion::from_quaternion(q1);
120        let q2 = Quaternion::new(0.9848078f32, 0.1736482f32, 0f32, 0f32);
121        let q2 = UnitQuaternion::from_quaternion(q2);
122        avg.add_quaternion(&q1);
123        avg.add_quaternion(&q2);
124
125        let qr = Quaternion::new(0.9914449f32, 0.1305262f32, 0f32, 0f32);
126        let qr = UnitQuaternion::from_quaternion(qr);
127        let q = avg.calc_average();
128
129        assert_relative_eq!(q, qr, max_relative = 0.00001);
130    }
131
132    #[test]
133    fn avg2_64() {
134        let mut avg = QuaternionAverager::new();
135        let q1 = Quaternion::new(0.9961947f64, 0.0871557f64, 0f64, 0f64);
136        let q1 = UnitQuaternion::from_quaternion(q1);
137        let q2 = Quaternion::new(0.9848078f64, 0.1736482f64, 0f64, 0f64);
138        let q2 = UnitQuaternion::from_quaternion(q2);
139        avg.add_quaternion(&q1);
140        avg.add_quaternion(&q2);
141
142        let qr = Quaternion::new(0.9914449f64, 0.1305262f64, 0f64, 0f64);
143        let qr = UnitQuaternion::from_quaternion(qr);
144        let q = avg.calc_average();
145
146        assert_relative_eq!(q, qr, max_relative = 0.000001);
147    }
148}
quaternion_averager/lib.rs

quaternion_averager/
lib.rs
