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

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      09/04/03 14:30:39

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

Log message:
        added lie group representation for SL(n), nxn matrices with det() = 1. 
a simple implementation for the matrix exponential in helpers supports it.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/helpers.h?cvsroot=toon&r1=1.39&r2=1.40
http://cvs.savannah.gnu.org/viewcvs/TooN/sl.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/TooN/test/sl.cc?cvsroot=toon&rev=1.1

Patches:
Index: helpers.h
===================================================================
RCS file: /cvsroot/toon/TooN/helpers.h,v
retrieving revision 1.39
retrieving revision 1.40
diff -u -b -r1.39 -r1.40
--- helpers.h   31 Mar 2009 04:30:11 -0000      1.39
+++ helpers.h   3 Apr 2009 14:30:31 -0000       1.40
@@ -195,6 +195,69 @@
        static Operator<Internal::Zero> Zero;
        static Operator<Internal::Identity> Identity;
 
+       /// row sum norm of input matrix m
+       /// computes the maximum of the sums of absolute values over rows
+       template <int R, int C, typename P, typename B>
+       P inline norm_inf( const Matrix<R,C,P,B> & m ){
+               using std::abs;
+               using std::max;
+               P n = 0;
+               for(int r = 0; r < m.num_rows(); ++r){
+                       P s = 0;
+                       for(int c = 0; c < m.num_cols(); ++c)
+                               s += abs(m(r,c));
+                       n = max(n,s);
+               }
+               return n;
+       }
 
+       /// col sum norm of input matrix m
+       /// computes the maximum of the sums of absolute values over columns
+       template <int R, int C, typename P, typename B>
+       P inline norm_1( const Matrix<R,C,P,B> & m ){
+               using std::abs;
+               using std::max;
+               P n = 0;
+               for(int c = 0; c < m.num_cols(); ++c){
+                       P s = 0;
+                       for(int r = 0; r < m.num_rows(); ++r)
+                               s += abs(m(r,c));
+                       n = max(n,s);
+               }
+               return n;
+       }
+
+       namespace Internal {
+               template <int R, int C, typename P, typename B>
+               inline Matrix<R, C, P> exp_taylor( const Matrix<R,C,P,B> & m ){
+                       SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
+                       Matrix<R,C,P> result = TooN::Zero(m.num_rows(), 
m.num_cols());
+                       Matrix<R,C,P> f = TooN::Identity(m.num_rows());
+                       P k = 1;
+                       while(norm_inf((result+f)-result) > 0){
+                               result += f;
+                               f = (m * f) / k;
+                               k += 1;
+                       }
+                       return result;
+               }
+       };
+       
+       /// computes the matrix exponential of a matrix m by 
+       /// scaling m by 1/(powers of 2), using Taylor series and 
+       /// squaring again.
+       /// @param m input matrix, must be square
+       /// @return result matrix of the same size/type as input
+       template <int R, int C, typename P, typename B>
+       inline Matrix<R, C, P> exp( const Matrix<R,C,P,B> & m ){
+               using std::max;
+               SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
+               const P l = log2(norm_inf(m));
+               const int s = max(0,(int)ceil(l));
+               Matrix<R,C,P> result = Internal::exp_taylor(m/(1<<s));
+               for(int i = 0; i < s; ++i)
+                       result = result * result;
+               return result;
+       }
 }
 #endif

Index: sl.h
===================================================================
RCS file: sl.h
diff -N sl.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ sl.h        3 Apr 2009 14:30:31 -0000       1.1
@@ -0,0 +1,163 @@
+/*                       
+        Copyright (C) 2005,2009 Tom Drummond, G. Reitmayr
+
+     This library is free software; you can redistribute it and/or
+     modify it under the terms of the GNU Lesser General Public
+     License as published by the Free Software Foundation; either
+     version 2.1 of the License, or (at your option) any later version.
+
+     This library is distributed in the hope that it will be useful,
+     but WITHOUT ANY WARRANTY; without even the implied warranty of
+     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+     Lesser General Public License for more details.
+
+     You should have received a copy of the GNU Lesser General Public
+     License along with this library; if not, write to the Free Software
+     Foundation, Inc.
+     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+#ifndef TOON_INCLUDE_SL_H
+#define TOON_INCLUDE_SL_H
+
+#include <TooN/TooN.h>
+#include <TooN/helpers.h>
+#include <TooN/LU.h>
+
+namespace TooN {
+
+template <int N, typename P> class SL;
+template <int N, typename P> std::istream & operator>>(std::istream &, SL<N, 
P> &);
+
+/// represents an element from the group SL(n), the nxn matrices M with det(M) 
= 1.
+/// This can be used to conveniently estimate homographies on n-1 dimentional 
spaces.
+/// The implementation uses the matrix exponential function @ref exp for
+/// exponentiation from an element in the Lie algebra and LU to compute an 
inverse.
+/// 
+/// The Lie algebra are the nxn matrices M with trace(M) = 0. The generators 
used
+/// to represent this vector space are the following:
+/// @item n-1 diag(...,1,-1,...), along the diagonal
+/// @item symmetric generators for every pair of off-diagonal elements
+/// @item anti-symmetric generators for every pair of off-diagonal elements
+/// This choice represents the fact that SL(n) can be interpreted as the 
product
+/// of all symmetric matrices with det() = 1 times SO(n).
+template <int N, typename Precision = double>
+class SL {
+       friend std::istream & operator>> <N,Precision>(std::istream &, SL &);
+public:
+       static const int size = N;
+       static const int dim = N*N - 1;
+
+       SL() : my_matrix(Identity) {}
+       template <int S, typename P, typename B>
+       SL( const Vector<S,P,B> & v ) { *this = exp(v); }
+
+       template <int R, int C, typename P, typename A>
+       SL(Matrix<R,C,P,A>& M) : my_matrix(M) {
+               coerce(M);
+       }
+
+       const Matrix<N,N,Precision> & get_matrix() const { return my_matrix; }
+       SL inverse() const { return SL(*this, Invert()); }
+
+       SL operator*( const SL & rhs) const { return SL(*this, rhs); }
+       SL operator*=( const SL & rhs) { *this = *this*rhs; return *this; }
+
+       template <int S, typename P, typename B>
+       static inline SL exp( const Vector<S,P,B> &);
+
+       static inline Matrix<N,N,Precision> generator(int);
+
+       template <int R, int C, typename P, typename A>
+       static void coerce(Matrix<R,C,P,A>& M){
+               using std::abs;
+               SizeMismatch<N,R>::test(N, M.num_rows());
+               SizeMismatch<N,C>::test(N, M.num_cols());
+               P det = LU<N>(M).determinant();
+               assert(abs(det) > 0);
+               M /= det;
+       }
+
+private:
+       struct Invert {};
+       SL( const SL & from, struct Invert ) : 
my_matrix(LU<N>(from.get_matrix()).get_inverse()) {}
+       SL( const SL & a, const SL & b) : my_matrix(a.get_matrix() * 
b.get_matrix()) {}
+
+       /// these constants indicate which parts of the parameter vector 
+       /// map to which generators
+       ///{
+       static const int COUNT_DIAG = N - 1;
+       static const int COUNT_SYMM = (dim - COUNT_DIAG)/2;
+       static const int COUNT_ASYMM = COUNT_SYMM;
+       static const int DIAG_LIMIT = COUNT_DIAG;
+       static const int SYMM_LIMIT = COUNT_SYMM + DIAG_LIMIT;
+       ///}
+
+       Matrix<N,N,Precision> my_matrix;
+};
+
+template <int N, typename Precision>
+template <int S, typename P, typename B>
+inline SL<N, Precision> SL<N, Precision>::exp( const Vector<S,P,B> & v){
+       SizeMismatch<S,dim>::test(v.size(), dim);
+       Matrix<N,N,Precision> t = Zero;
+       for(int i = 0; i < dim; ++i)
+               t += generator(i) * v[i];
+       SL<N, Precision> result;
+       result.my_matrix = TooN::exp(t);
+       return result;
+}
+
+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 = Zero;
+       if(i < DIAG_LIMIT) {                            // first ones are the 
diagonal ones
+               result(i,i) = 1;
+               result(i+1,i+1) = -1;
+       } else if(i < SYMM_LIMIT){                      // then the symmetric 
ones
+               int row = 0, col = i - DIAG_LIMIT + 1;
+               while(col > (N - row - 1)){
+                       col -= (N - row - 1); 
+                       ++row;
+               }
+               col += row;
+               result(row, col) = result(col, row) = 1;
+       } else {                                                        // 
finally the antisymmetric ones
+               int row = 0, col = i - SYMM_LIMIT + 1;
+               while(col > N - row - 1){
+                       col -= N - row - 1; 
+                       ++row;
+               }
+               col += row;
+               result(row, col) = -1;
+               result(col, row) = 1;
+       }
+       return result;
+}
+
+template <int S, typename PV, typename B, int N, typename P>
+Vector<N, P> operator*( const SL<N, P> & lhs, const Vector<S,PV,B> & rhs ){
+       return lhs.get_matrix() * rhs;
+}
+
+template <int S, typename PV, typename B, int N, typename P>
+Vector<N, P> operator*( const Vector<S,PV,B> & lhs, const SL<N,P> & rhs ){
+       return lhs * rhs.get_matrix();
+}
+
+template <int N, typename P>
+std::ostream & operator<<(std::ostream & out, const SL<N, P> & h){
+       out << h.get_matrix();
+       return out;
+}
+
+template <int N, typename P>
+std::istream & operator>>(std::istream & in, SL<N, P> & h){
+       in >> h.my_matrix;
+       SL<N,P>::coerce(h.my_matrix);
+       return in;
+}
+
+};
+
+#endif

Index: test/sl.cc
===================================================================
RCS file: test/sl.cc
diff -N test/sl.cc
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ test/sl.cc  3 Apr 2009 14:30:34 -0000       1.1
@@ -0,0 +1,26 @@
+#include <TooN/TooN.h>
+#include <TooN/sl.h>
+
+#include <iostream>
+#include <iomanip>
+
+using namespace TooN;
+using namespace std;
+
+int main(int , char ** ){
+       SL<3> h(makeVector(1,0,-1,0,0,0,0,0));
+       cout << h << endl;
+       cout << h.inverse() << endl;
+       cout << SL<3>::exp(makeVector(-1,0,1,0,0,0,0,0)) << endl;
+       cout << h * h.inverse() << endl;
+       
+       for(int i = 0; i < SL<3>::dim; ++i)
+               cout << "generator " << i << "\n" << SL<3>::generator(i) << 
endl;
+
+       for(int i = 0; i < SL<5>::dim; ++i)
+               cout << "generator " << i << "\n" << SL<5>::generator(i) << 
endl;
+       
+       cout << SL<2>::exp(makeVector(1,2,3)) << endl;
+       
+       return 0;
+}