Merge pull request #156 from dshizzle/master

Improved QR algorithm convergence

src/lib.rs

@@ -151,7 +151,8 @@ pub use structs::{
 pub use linalg::{
     qr,
     householder_matrix,
-    cholesky
+    cholesky,
+    hessenberg
 };
 
 mod structs;

src/linalg/decompositions.rs

@@ -1,5 +1,5 @@
 use traits::operations::{Transpose, ApproxEq};
-use traits::structure::{ColSlice, Eye, Indexable, Diag, SquareMat, BaseFloat};
+use traits::structure::{ColSlice, Eye, Indexable, Diag, SquareMat, BaseFloat, Cast};
 use traits::geometry::Norm;
 use std::cmp::min;
 use std::ops::{Mul, Add, Sub};
@@ -71,7 +71,7 @@ pub fn qr<N, V, M>(m: &M) -> (M, M)
     (q, r)
 }
 
-/// Eigendecomposition of a square matrix using the qr algorithm.
+/// Eigendecomposition of a square symmetric matrix using the qr algorithm
 pub fn eigen_qr<N, V, VS, M>(m: &M, eps: &N, niter: usize) -> (M, V)
     where N:  BaseFloat,
           V:  Mul<M, Output = V>,
@@ -79,39 +79,165 @@ pub fn eigen_qr<N, V, VS, M>(m: &M, eps: &N, niter: usize) -> (M, V)
           M:  Indexable<(usize, usize), N> + SquareMat<N, V> + Add<M, Output = M> +
               Sub<M, Output = M> + ColSlice<VS> +
               ApproxEq<N> + Copy {
-    let mut eigenvectors: M = ::one::<M>();
+
-    let mut eigenvalues = *m;
+    let (mut eigenvectors, mut eigenvalues) = hessenberg(m);
-    // let mut shifter: M = Eye::new_identity(rows);
+
+    // Allocate arrays for Givens rotation components
+    let mut c = Vec::<N>::with_capacity(::dim::<M>()-1);
+    let mut s = Vec::<N>::with_capacity(::dim::<M>()-1);
+
+    if ::dim::<M>() == 1 {
+        return (eigenvectors, eigenvalues.diag());
+    }
+
+    unsafe { 
+        c.set_len(::dim::<M>()-1);
+        s.set_len(::dim::<M>()-1);
+    }
 
     let mut iter = 0;
-    for _ in 0..niter {
+    let mut curdim = ::dim::<M>()-1;
-        let mut stop = true;
+
+    for _ in 0..::dim::<M>() {
+
+        let mut stop = false;
+
+        while !stop && iter < niter {
+
+            let lambda;
+
+            unsafe {
+                let a = eigenvalues.unsafe_at((curdim-1, curdim-1));
+                let b = eigenvalues.unsafe_at((curdim-1, curdim));
+                let c = eigenvalues.unsafe_at((curdim, curdim-1));
+                let d = eigenvalues.unsafe_at((curdim, curdim));
+
+                let trace = a + d;
+                let det = a * d - b * c;
+
+                let constquarter: N = Cast::from(0.25f64);
+                let consthalf: N = Cast::from(0.5f64);
+
+                let e = (constquarter * trace * trace - det).sqrt();
+
+                let lambda1 = consthalf * trace + e;
+                let lambda2 = consthalf * trace - e;
+
+                if (lambda1 - d).abs() < (lambda2 - d).abs() {
+                    lambda = lambda1;
+                }
+                else {
+                    lambda = lambda2;
+                }
+
+            }
+
+            // Shift matrix
+            for k in 0..curdim+1 {
+                unsafe { 
+                    let a = eigenvalues.unsafe_at((k,k));
+                    eigenvalues.unsafe_set((k,k), a - lambda);
+                }
+            }
+            
+
+            // Givens rotation from left
+            for k in 0..curdim {
+                let x_i = unsafe { eigenvalues.unsafe_at((k,k)) };
+                let x_j = unsafe { eigenvalues.unsafe_at((k+1,k)) };
+                let r = (x_i*x_i + x_j*x_j).sqrt();
+                let ctmp = x_i / r;
+                let stmp = -x_j / r;
+
+                c[k] = ctmp;
+                s[k] = stmp;
+
+                for j in k..(curdim+1) {
+
+                    unsafe {
+                        let a = eigenvalues.unsafe_at((k,j));
+                        let b = eigenvalues.unsafe_at((k+1,j));
+
+                        eigenvalues.unsafe_set((k,j), ctmp * a - stmp * b);
+                        eigenvalues.unsafe_set((k+1,j), stmp * a + ctmp * b);
+                    }
 
-        for j in 0..::dim::<M>() {
-            for i in 0..j {
-                if unsafe { eigenvalues.unsafe_at((i, j)) }.abs() >= *eps {
-                    stop = false;
-                    break;
                 }
             }
 
-            for i in j + 1..::dim::<M>() {
+            // Givens rotation from right applied to eigenvalues
-                if unsafe { eigenvalues.unsafe_at((i, j)) }.abs() >= *eps {
+            for k in 0..curdim {
+                for i in 0..(k+2) {
+
+                    unsafe {
+                        let a = eigenvalues.unsafe_at((i,k));
+                        let b = eigenvalues.unsafe_at((i,k+1));
+
+
+                        eigenvalues.unsafe_set((i,k), c[k] * a - s[k] * b);
+                        eigenvalues.unsafe_set((i,k+1), s[k] * a + c[k] * b);
+                    }
+                }
+
+            }
+
+            
+            // Shift back
+            for k in 0..curdim+1 {
+                unsafe { 
+                    let a = eigenvalues.unsafe_at((k,k));
+                    eigenvalues.unsafe_set((k,k), a + lambda);
+                }
+            }
+            
+
+            // Givens rotation from right applied to eigenvectors
+            for k in 0..curdim {
+                for i in 0..::dim::<M>() {
+
+                    unsafe {
+                        let a = eigenvectors.unsafe_at((i,k));
+                        let b = eigenvectors.unsafe_at((i,k+1));
+
+
+                        eigenvectors.unsafe_set((i,k), c[k] * a - s[k] * b);
+                        eigenvectors.unsafe_set((i,k+1), s[k] * a + c[k] * b);
+                    }
+                }
+            }
+
+            iter = iter + 1;
+
+            stop = true;
+
+            for j in 0..curdim {
+
+                // Check last row
+                if unsafe { eigenvalues.unsafe_at((curdim, j)) }.abs() >= *eps {
+                    stop = false;
+                    break;
+                }
+
+                // Check last column
+                if unsafe { eigenvalues.unsafe_at((j, curdim)) }.abs() >= *eps {
                     stop = false;
                     break;
                 }
             }
         }
 
+
         if stop {
-            break;
+
+            if curdim > 1 {
+                curdim = curdim - 1;
+            }
+            else {
+                break;
+            }
+
         }
-        iter = iter + 1;
 
-        let (q, r) = qr(&eigenvalues);;
-
-        eigenvalues  = r * q;
-        eigenvectors = eigenvectors * q;
     }
 
     (eigenvectors, eigenvalues.diag())
@@ -164,3 +290,48 @@ pub fn cholesky<N, V, VS, M>(m: &M) -> Result<M, &'static str>
 
     return Ok(out);
 }
+
+/// Hessenberg
+/// Returns the matrix m in Hessenberg form and the corresponding similarity transformation
+///
+/// # Arguments
+/// * `m` - matrix to transform
+///
+/// # Returns
+/// * First return value `q` - Similarity matrix p such that q * h * q^T = m
+/// * Second return value `h` - Matrix m in Hessenberg form
+pub fn hessenberg<N, V, M>(m: &M) -> (M, M)
+    where N: BaseFloat,
+          V: Indexable<usize, N> + Norm<N>,
+          M: Copy + Eye + ColSlice<V> + Transpose + Indexable<(usize, usize), N> +
+             Mul<M, Output = M> {
+    
+    let mut h = m.clone();
+    let (rows, cols) = h.shape();
+
+    let mut q : M = Eye::new_identity(cols);
+
+    if cols <= 2 {
+        return (q, h);
+    }
+
+    for ite in 0..(cols-2) {
+        let mut v = h.col_slice(ite, ite+1, rows);
+
+        let alpha = Norm::norm(&v);
+
+        unsafe {
+            let x = v.unsafe_at(0);
+            v.unsafe_set(0, x - alpha);
+        }
+
+        if !::is_zero(&v.normalize_mut()) {
+            let p: M = householder_matrix(rows, ite+1, v);
+
+            q = q * p;
+            h = p * h * p;
+        }
+    }
+
+    return (q, h);
+}

src/linalg/mod.rs

@@ -1,4 +1,4 @@
 
-pub use self::decompositions::{qr, eigen_qr, householder_matrix, cholesky};
+pub use self::decompositions::{qr, eigen_qr, householder_matrix, cholesky, hessenberg};
 
 mod decompositions;

tests/mat.rs

@@ -3,7 +3,7 @@ extern crate rand;
 
 use rand::random;
 use na::{Vec1, Vec3, Mat1, Mat2, Mat3, Mat4, Mat5, Mat6, Rot2, Rot3, Persp3, PerspMat3, Ortho3,
-         OrthoMat3, DMat, DVec, Row, Col, BaseFloat, Diag, Transpose, RowSlice, ColSlice};
+         OrthoMat3, DMat, DVec, Row, Col, BaseFloat, Diag, Transpose, RowSlice, ColSlice, Shape};
 
 macro_rules! test_inv_mat_impl(
   ($t: ty) => (
@@ -63,28 +63,49 @@ macro_rules! test_cholesky_impl(
   );
 );
 
-// NOTE: deactivated untile we get a better convergence rate.
+macro_rules! test_hessenberg_impl(
-// macro_rules! test_eigen_qr_impl(
+  ($t: ty) => (
-//     ($t: ty) => {
+    for _ in (0usize .. 10000) {
-//         for _ in (0usize .. 10000) {
+      
-//             let randmat : $t = random();
+      let randmat : $t = random();
-//             // Make it symetric so that we can recompose the matrix to test at the end.
+
-//             let randmat = na::transpose(&randmat) * randmat;
+      let (q, h) = na::hessenberg(&randmat);
-// 
+      let recomp = q * h * na::transpose(&q);
-//             let (eigenvectors, eigenvalues) = na::eigen_qr(&randmat, &Float::epsilon(), 100);
+
-// 
+      let (rows, cols) = h.shape();
-//             let diag: $t = Diag::from_diag(&eigenvalues);
+
-// 
+      // Check if `h` has zero entries below the first subdiagonal
-//             let recomp = eigenvectors * diag * na::transpose(&eigenvectors);
+      if cols > 2 {
-// 
+          for j in 0..(cols-2) {
-//             println!("eigenvalues: {}", eigenvalues);
+              for i in (j+2)..rows {
-//             println!("   mat: {}", randmat);
+                  assert!(na::approx_eq(&h[(i,j)], &0.0f64));
-//             println!("recomp: {}", recomp);
+              }
-// 
+          }
-//             assert!(na::approx_eq_eps(&randmat,  &recomp, &1.0e-2));
+      }
-//         }
+
-//     }
+      assert!(na::approx_eq(&randmat,  &recomp));
-// )
+    }
+  );
+);
+
+macro_rules! test_eigen_qr_impl(
+    ($t: ty) => {
+        for _ in (0usize .. 10000) {
+            let randmat : $t = random();
+            // Make it symetric so that we can recompose the matrix to test at the end.
+            let randmat = na::transpose(&randmat) * randmat;
+            let (eigenvectors, eigenvalues) = na::eigen_qr(&randmat, &1e-13, 100);
+ 
+            let diag: $t = Diag::from_diag(&eigenvalues);
+            let recomp = eigenvectors * diag * na::transpose(&eigenvectors);
+            println!("eigenvalues: {:?}", eigenvalues);
+            println!("   mat: {:?}", randmat);
+            println!("recomp: {:?}", recomp);
+
+            assert!(na::approx_eq_eps(&randmat,  &recomp, &1.0e-2));
+        }
+    }
+);
 
 #[test]
 fn test_transpose_mat1() {
@@ -532,36 +553,35 @@ fn test_qr_mat6() {
     test_qr_impl!(Mat6<f64>);
 }
 
-// NOTE: deactivated until we get a better convergence rate.
+#[test]
-// #[test]
+fn test_eigen_qr_mat1() {
-// fn test_eigen_qr_mat1() {
+    test_eigen_qr_impl!(Mat1<f64>);
-//     test_eigen_qr_impl!(Mat1<f64>);
+}
-// }
+
-// 
+#[test]
-// #[test]
+fn test_eigen_qr_mat2() {
-// fn test_eigen_qr_mat2() {
+    test_eigen_qr_impl!(Mat2<f64>);
-//     test_eigen_qr_impl!(Mat2<f64>);
+}
-// }
+
-// 
+#[test]
-// #[test]
+fn test_eigen_qr_mat3() {
-// fn test_eigen_qr_mat3() {
+    test_eigen_qr_impl!(Mat3<f64>);
-//     test_eigen_qr_impl!(Mat3<f64>);
+}
-// }
+
-// 
+#[test]
-// #[test]
+fn test_eigen_qr_mat4() {
-// fn test_eigen_qr_mat4() {
+    test_eigen_qr_impl!(Mat4<f64>);
-//     test_eigen_qr_impl!(Mat4<f64>);
+}
-// }
+
-// 
+#[test]
-// #[test]
+fn test_eigen_qr_mat5() {
-// fn test_eigen_qr_mat5() {
+    test_eigen_qr_impl!(Mat5<f64>);
-//     test_eigen_qr_impl!(Mat5<f64>);
+}
-// }
+
-// 
+#[test]
-// #[test]
+fn test_eigen_qr_mat6() {
-// fn test_eigen_qr_mat6() {
+    test_eigen_qr_impl!(Mat6<f64>);
-//     test_eigen_qr_impl!(Mat6<f64>);
+}
-// }
 
 #[test]
 fn test_from_fn() {
@@ -733,6 +753,36 @@ fn test_cholesky_mat6() {
     test_cholesky_impl!(Mat6<f64>);
 }
 
+#[test]
+fn test_hessenberg_mat1() {
+    test_hessenberg_impl!(Mat1<f64>);
+}
+
+#[test]
+fn test_hessenberg_mat2() {
+    test_hessenberg_impl!(Mat2<f64>);
+}
+
+#[test]
+fn test_hessenberg_mat3() {
+    test_hessenberg_impl!(Mat3<f64>);
+}
+
+#[test]
+fn test_hessenberg_mat4() {
+    test_hessenberg_impl!(Mat4<f64>);
+}
+
+#[test]
+fn test_hessenberg_mat5() {
+    test_hessenberg_impl!(Mat5<f64>);
+}
+
+#[test]
+fn test_hessenberg_mat6() {
+    test_hessenberg_impl!(Mat6<f64>);
+}
+
 #[test]
 fn test_transpose_square_mat() {
     let col_major_mat = &[0, 1, 2, 3,