[Toon-members] TooN helpers.h sl.h test/sl.cc test/log.cc

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      11/03/31 21:04:44

Modified files:
        .              : helpers.h sl.h 
        test           : sl.cc 
Added files:
        test           : log.cc 

Log message:
        added implementations for general matrix square root and matrix 
logarithm. Added SL<>::ln() based on the matrix logarithm function. no 
guarantees about numerical stability are given :)

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/helpers.h?cvsroot=toon&r1=1.91&r2=1.92
http://cvs.savannah.gnu.org/viewcvs/TooN/sl.h?cvsroot=toon&r1=1.8&r2=1.9
http://cvs.savannah.gnu.org/viewcvs/TooN/test/sl.cc?cvsroot=toon&r1=1.4&r2=1.5
http://cvs.savannah.gnu.org/viewcvs/TooN/test/log.cc?cvsroot=toon&rev=1.1

Patches:
Index: helpers.h
===================================================================
RCS file: /cvsroot/toon/TooN/helpers.h,v
retrieving revision 1.91
retrieving revision 1.92
diff -u -b -r1.91 -r1.92
--- helpers.h   3 May 2010 17:37:06 -0000       1.91
+++ helpers.h   31 Mar 2011 21:04:43 -0000      1.92
@@ -33,6 +33,7 @@
 #define TOON_INCLUDE_HELPERS_H
 
 #include <TooN/TooN.h>
+#include <TooN/gaussian_elimination.h>
 #include <cmath>
 #include <functional>
 #include <utility>
@@ -272,6 +273,25 @@
                        }
                        return result;
                }
+
+               ///@internal
+               ///@brief Logarithm of a matrix using a the Taylor series
+               ///This will not work if the norm of the matrix is too large.
+               template <int R, int C, typename P, typename B>
+               inline Matrix<R, C, P> log_taylor( const Matrix<R,C,P,B> & m ){
+                       TooN::SizeMismatch<R, C>::test(m.num_rows(), 
m.num_cols());
+                       Matrix<R,C,P> X = m - TooN::Identity * 1.0;
+                       Matrix<R,C,P> F = X;
+                       Matrix<R,C,P> sum = TooN::Zeros(m.num_rows(), 
m.num_cols());
+                       P k = 1;
+                       while(norm_inf((sum+F/k)-sum) > 0){
+                               sum += F/k;
+                               F = -F*X;
+                               k += 1;
+                       }
+                       return sum;
+               }
+
        };
        
        /// computes the matrix exponential of a matrix m by 
@@ -292,6 +312,48 @@
                return result;
        }
        
+       /// computes a matrix square root of a matrix m by
+       /// the product form of the Denman and Beavers iteration
+       /// as given in Chen et al. 'Approximating the logarithm of a matrix to 
specified accuracy', 
+       /// J. Matrix Anal Appl, 2001. This is used for the matrix
+       /// logarithm function, but is useable by on its own.
+       /// @param m input matrix, must be square
+       /// @return a square root of m of the same size/type as input
+       /// @ingroup gLinAlg
+       template <int R, int C, typename P, typename B>
+       inline Matrix<R, C, P> sqrt( const Matrix<R,C,P,B> & m){
+               SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
+               Matrix<R,C,P> M = m;
+               Matrix<R,C,P> Y = m;
+               Matrix<R,C,P> M_inv(m.num_rows(), m.num_cols());
+               const Matrix<R,C,P> id = Identity(m.num_rows());
+               do {
+                       M_inv = gaussian_elimination(M, id);
+                       Y = Y * (id + M_inv) * 0.5;
+                       M = 0.5 * (id + (M + M_inv) * 0.5);
+               } while(norm_inf(M - M_inv) > 0);
+               return Y;
+       }
+       
+       /// computes the matrix logarithm of a matrix m using the inverse 
scaling and 
+       /// squaring method. The overall approach is described in
+       /// Chen et al. 'Approximating the logarithm of a matrix to specified 
accuracy', 
+       /// J. Matrix Anal Appl, 2001, but this implementation only uses a 
simple
+       /// taylor series after the repeated square root operation.
+       /// @param m input matrix, must be square
+       /// @return the log of m of the same size/type as input
+       /// @ingroup gLinAlg
+       template <int R, int C, typename P, typename B>
+       inline Matrix<R, C, P> log( const Matrix<R,C,P,B> & m){
+               int counter = 0;
+               Matrix<R,C,P> A = m;
+               while(norm_inf(A - Identity*1.0) > 0.5){
+                       ++counter;
+                       A = sqrt(A);
+               }
+               return Internal::log_taylor(A) * pow(2.0, counter);
+       }
+       
        /// Returns true if every element is finite
        ///@ingroup gLinAlg
        template<int S, class P, class B> bool isfinite(const Vector<S, P, B>& 
v)

Index: sl.h
===================================================================
RCS file: /cvsroot/toon/TooN/sl.h,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- sl.h        19 Dec 2009 05:56:14 -0000      1.8
+++ sl.h        31 Mar 2011 21:04:43 -0000      1.9
@@ -88,6 +88,8 @@
        template <int S, typename P, typename B>
        static inline SL exp( const Vector<S,P,B> &);
 
+       inline Vector<N*N-1, Precision> ln() const ;
+
        /// returns one generator of the group. see SL for a detailed 
description of 
        /// the generators used.
        /// @arg i number of the generator between 0 and SL::dim -1 inclusive
@@ -96,7 +98,7 @@
 private:
        struct Invert {};
        SL( const SL & from, struct Invert ) {
-               Matrix<N> id = Identity;
+               const Matrix<N> id = Identity;
                my_matrix = gaussian_elimination(from.my_matrix, id);
        }
        SL( const SL & a, const SL & b) : my_matrix(a.get_matrix() * 
b.get_matrix()) {}
@@ -135,6 +137,28 @@
 }
 
 template <int N, typename Precision>
+inline Vector<N*N-1, Precision> SL<N, Precision>::ln() const {
+       const Matrix<N> l = TooN::log(my_matrix);
+       Vector<SL<N,Precision>::dim, Precision> v;
+       Precision last = 0;
+       for(int i = 0; i < DIAG_LIMIT; ++i){    // diagonal elements
+               v[i] = l(i,i) + last;
+               last = l(i,i);
+       }
+       for(int i = DIAG_LIMIT, row = 0, col = 1; i < SYMM_LIMIT; ++i) {        
// calculate symmetric and antisymmetric in one go
+               // do the right thing here to calculate the correct indices !
+               v[i] = (l(row, col) + l(col, row))*0.5;
+               v[i+COUNT_SYMM] = (-l(row, col) + l(col, row))*0.5;
+               ++col;
+               if( col == N ){
+                       ++row;
+                       col = row+1;
+               }
+       }
+       return v;
+}
+
+template <int N, typename Precision>
 inline Matrix<N,N,Precision> SL<N, Precision>::generator(int i){
        assert( i > -1 && i < dim );
        Matrix<N,N,Precision> result(Zeros);

Index: test/sl.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/sl.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- test/sl.cc  18 Apr 2010 09:49:18 -0000      1.4
+++ test/sl.cc  31 Mar 2011 21:04:44 -0000      1.5
@@ -43,6 +43,9 @@
        
        cout << endl;
        
+       cout << "log(h)\t" << h.ln() << endl;
+       cout << "diff\n" << h.get_matrix() - SL<3>::exp(h.ln()).get_matrix() << 
endl;
+       
 /*
        SO3<> so3(makeVector(1,0,1));
        h = so3;

Index: test/log.cc
===================================================================
RCS file: test/log.cc
diff -N test/log.cc
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ test/log.cc 31 Mar 2011 21:04:44 -0000      1.1
@@ -0,0 +1,20 @@
+#include <TooN/TooN.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace TooN;
+
+int main(int argc, char ** argv){
+    Matrix<2> t;
+    t[0] = makeVector(10.0, 17.182818284590450);
+    t[1] = makeVector( 0.0, 27.182818284590450);
+    
+    Matrix<2> s = sqrt(t);
+    Matrix<2> l = log(t);
+    
+    cout << "input\n" << t << endl;
+    cout << "square root\n" << s << endl;
+    cout << "log\n" << l << endl;
+    cout << "difference exp(l) - t\n" << exp(l) - t << endl;
+}