[Toon-members] TooN wls.h

[Tom Drummond]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Tom Drummond <twd20>    09/04/10 07:57:15

Modified files:
        .              : wls.h 

Log message:
        WLS updated to TooN2 - one version for static and dynamic
        now templated on the decomposition class used to invert the inverse 
covariance matrix
        also add_df changed name to add_mJ (measurement, Jacobian)

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/wls.h?cvsroot=toon&r1=1.11&r2=1.12

Patches:
Index: wls.h
===================================================================
RCS file: /cvsroot/toon/TooN/wls.h,v
retrieving revision 1.11
retrieving revision 1.12
diff -u -b -r1.11 -r1.12
--- wls.h       10 Apr 2009 05:41:28 -0000      1.11
+++ wls.h       10 Apr 2009 07:57:14 -0000      1.12
@@ -31,10 +31,9 @@
 #define __WLS_H
 
 #include <TooN/TooN.h>
-#include <TooN/SVD.h>
+#include <TooN/Cholesky.h>
 #include <TooN/helpers.h>
 
-#include <cassert>
 #include <cmath>
 
 #ifndef TOON_NO_NAMESPACE
@@ -43,30 +42,35 @@
 
 /// Performs weighted least squares computation.
 /// @param Size The number of dimensions in the system
+/// @param Precision The numerical precision used (double, float etc)
+/// @param Decomposition The class used to invert the inverse Covariance 
matrix (must have two size and one precision template arguments)
 /// @ingroup gEquations
-template <int Size=-1>
+template <int Size=Dynamic, class Precision=double,
+                 template<int Rows, int Cols, class Precision> class 
Decomposition = Cholesky>
 class WLS {
 public:
-       /// Default constructor
-  WLS(){clear();}
-  /// Construct using a given regularisation prior
-  WLS(double prior){clear(prior);}
+
+       /// Default constructor or construct with the number of dimensions for 
the Dynamic case
+       WLS(int size=0) :
+               my_C_inv(size,size),
+               my_vector(size)
+       {
+               clear();
+       }
 
   /// Clear all the measurements and apply a constant regularisation term. 
   /// Equates to a prior that says all the parameters are zero with 
\f$\sigma^2 = \frac{1}{\text{val}}\f$.
   /// @param prior The strength of the prior
-  void clear(double prior=0){
-    Identity(my_C_inv,prior);
-    for(int i=0; i<Size; i++){
-      my_vector[i]=0;
-    }
+       void clear(){
+               my_C_inv = Zero;
+               my_vector = Zero;
   }
 
        /// Applies a constant regularisation term. 
        /// Equates to a prior that says all the parameters are zero with 
\f$\sigma^2 = \frac{1}{\text{val}}\f$.
        /// @param val The strength of the prior
-  void add_prior(double val){
-    for(int i=0; i<Size; i++){
+       void add_prior(Precision val){
+               for(int i=0; i<my_C_inv.num_rows(); i++){
       my_C_inv(i,i)+=val;
     }
   }
@@ -74,9 +78,10 @@
        /// Applies a regularisation term with a different strength for each 
parameter value. 
        /// Equates to a prior that says all the parameters are zero with 
\f$\sigma_i^2 = \frac{1}{\text{v}_i}\f$.
        /// @param v The vector of priors
-  template<class Accessor>
-  void add_prior(const FixedVector<Size,Accessor>& v){
-    for(int i=0; i<Size; i++){
+       template<class B2>
+       void add_prior(const Vector<Size,Precision,B2>& v){
+               SizeMismatch<Size,Size>::test(my_C_inv.num_rows(), v.size());
+               for(int i=0; i<my_C_inv.num_rows(); i++){
       my_C_inv(i,i)+=v[i];
     }
   }
@@ -84,8 +89,8 @@
        /// Applies a whole-matrix regularisation term. 
        /// This is the same as adding the \f$m\f$ to the inverse covariance 
matrix.
        /// @param m The inverse covariance matrix to add
-  template<class Accessor>
-  void add_prior(const FixedMatrix<Size,Size,Accessor>& m){
+       template<class B2>
+       void add_prior(const Matrix<Size,Size,Precision,B2>& m){
     my_C_inv+=m;
   }
 
@@ -93,15 +98,11 @@
        /// @param m The value of the measurement
        /// @param J The Jacobian for the measurement 
\f$\frac{\partial\text{m}}{\partial\text{param}_i}\f$
        /// @param weight The inverse variance of the measurement (default = 1)
-  template<class Accessor>
-  inline void add_df(double m, const FixedVector<Size,Accessor>& J, double 
weight = 1) {
-    Vector<Size> Jw = J*weight;
-    for(int i=0; i<Size; i++){
-      for(int j=0; j<Size; j++){
-       my_C_inv[i][j]+=J[i]*Jw[j];
-      }
-      my_vector[i]+=Jw[i]*m;
-    }
+       template<class B2>
+       inline void add_mJ(Precision m, const Vector<Size, Precision, B2>& J, 
Precision weight = 1) {
+               Vector<Size,Precision> Jw = J*weight;
+               my_C_inv += Jw.as_col() * J.as_row();
+               my_vector+= m*Jw;
   }
 
        /// Add multiple measurements at once (much more efficiently)
@@ -109,17 +110,21 @@
        /// @param m The measurements to add
        /// @param J The Jacobian matrix 
\f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
        /// @param invcov The inverse covariance of the measurement values
-  template<int N, class Accessor1, class Accessor2, class Accessor3>
-  inline void add_df(const FixedVector<N,Accessor1>& m,
-                    const FixedMatrix<Size,N,Accessor2>& J,
-                    const FixedMatrix<N,N,Accessor3>& invcov){
-    my_C_inv += J * invcov * J.T();
-    my_vector += J * invcov * m;
+       template<int N, class B1, class B2, class B3>
+       inline void add_mJ(const Vector<N,Precision,B1>& m,
+                                          const Matrix<Size,N,Precision,B2>& J,
+                                          const Matrix<N,N,Precision,B3>& 
invcov){
+               Matrix<Size,N,Precision> temp =  J * invcov;
+               my_C_inv += temp * J.T();
+               my_vector += temp * m;
   }
 
+
+       /// Process all the measurements and compute the weighted least squares 
set of parameter values
+       /// stores the result internally which can then be accessed by calling 
get_mu()
   void compute(){
-    my_svd.compute(my_C_inv);
-    my_mu=my_svd.backsub(my_vector);
+               my_decomposition.compute(my_C_inv);
+               my_mu=my_decomposition.backsub(my_vector);
   }
 
   /// Combine measurements from two WLS systems
@@ -130,180 +135,28 @@
   }
 
   /// Returns the inverse covariance matrix
-  Matrix<Size,Size,RowMajor>& get_C_inv() {return my_C_inv;}
-  /// Returns the inverse covariance matrix
-  const Matrix<Size,Size,RowMajor>& get_C_inv() const {return my_C_inv;}
-  Vector<Size>& get_mu(){return my_mu;}
-  const Vector<Size>& get_mu() const {return my_mu;}
-  Vector<Size>& get_vector(){return my_vector;}
-  const Vector<Size>& get_vector() const {return my_vector;}
-  SVD<Size>& get_svd(){return my_svd;}
-  const SVD<Size>& get_svd() const {return my_svd;}
-
-private:
-  Vector<Size> my_mu;
-  Matrix<Size,Size,RowMajor> my_C_inv;
-  Vector<Size> my_vector;
-  SVD<Size,Size> my_svd;
-
-  // comment out to allow bitwise copying
-  WLS( WLS& copyof );
-  int operator = ( WLS& copyof );
-};
-
-
-
-
-
-
-
-//  syg21: Dynamic WLS
-
-/// Performs weighted least squares computation.
-/// @param Size The number of dimensions in the system
-/// @ingroup gMaths
-template<> 
-class WLS<-1> {
-public:
-  /// Default constructor 
-   //WLS(){clear();} 
-  WLS(unsigned int s) 
-    : Size(s), my_mu(Size), my_vector(Size), my_C_inv(Size, Size)
-    { 
-      clear();
-    } 
-     
-    void Identity(Matrix<> &M, double d)
-    {
-      for(int r=0; r<M.num_rows(); r++)
-       {
-         for(int c=0; c<M.num_cols(); c++)
-           {
-             M[r][c] = 0.0;
-           }
-         M[r][r] = 1.0;
-       }
-    }
-
-  /// Clear all the measurements and apply a constant regularisation term. 
-  /// Equates to a prior that says all the parameters are zero with 
\f$\sigma^2 = \frac{1}{\text{val}}\f$.
-  /// @param prior The strength of the prior
-  void clear(double prior=0){
-    Identity(my_C_inv,prior);
-    for(int i=0; i<Size; i++){
-      my_vector[i]=0;
-    }
-  }
-
-  /// Applies a constant regularisation term. 
-  /// Equates to a prior that says all the parameters are zero with 
\f$\sigma^2 = \frac{1}{\text{val}}\f$.
-  /// @param val The strength of the prior
-  void add_prior(double val){
-      for(int i=0; i<Size; i++){
-      my_C_inv(i,i)+=val;
-    }
-  }
-  
-  /// Applies a regularisation term with a different strength for each 
parameter value. 
-  /// Equates to a prior that says all the parameters are zero with 
\f$\sigma_i^2 = \frac{1}{\text{v}_i}\f$.
-  /// @param v The vector of priors
-  template<int VSize, class Accessor>
-  void add_prior(const FixedVector<VSize,Accessor>& v){
-    assert(VSize==Size);
-    for(int i=0; i<VSize; i++){
-      my_C_inv(i,i)+=v[i];
-    }
-  }
-  /// Applies a whole-matrix regularisation term. 
-  /// This is the same as adding the \f$m\f$ to the inverse covariance matrix.
-  /// @param m The inverse covariance matrix to add
-  template<int MSize, class Accessor>
-  void add_prior(const FixedMatrix<MSize,MSize,Accessor>& m){
-    my_C_inv+=m;
-  }
-
-  /// Add a single measurement 
-  /// @param m The value of the measurement
-  /// @param J The Jacobian for the measurement 
\f$\frac{\partial\text{m}}{\partial\text{param}_i}\f$
-  /// @param weight The inverse variance of the measurement (default = 1)
-  template<int VSize, class Accessor>
-  inline void add_df(double m, const FixedVector<VSize,Accessor>& J, double 
weight = 1) {
-    assert(VSize==Size);
-    Vector<VSize> Jw = J*weight;
-    for(int i=0; i<VSize; i++){
-      for(int j=0; j<VSize; j++){
-       my_C_inv[i][j]+=J[i]*Jw[j];
-      }
-      my_vector[i]+=Jw[i]*m;
-    }
-  }
-
-  /// Add a single measurement 
-  /// @param m The value of the measurement
-  /// @param J The Jacobian for the measurement 
\f$\frac{\partial\text{m}}{\partial\text{param}_i}\f$
-  /// @param weight The inverse variance of the measurement (default = 1)
-  template<class Accessor>
-    inline void add_df(double m, const DynamicVector<Accessor>& J, double 
weight = 1) {
-    Vector<> Jw(Size);
-    Jw = J * weight;
-    for(int i=0; i<Size; i++){
-      for(int j=0; j<Size; j++){
-       my_C_inv[i][j]+=J[i]*Jw[j];
-      }
-      my_vector[i]+=Jw[i]*m;
-    }
-  }
-
-
-  void compute(){
-    //  create SVD
-    SVD<> my_svd(my_C_inv);
-    //my_svd.compute(my_C_inv);
-    my_mu=my_svd.backsub(my_vector);
-  }
-
-  /// Returns mu
-  const Vector<>& get_mu() const {return my_mu;}
-  Vector<>& get_mu() {return my_mu;}
+       Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;}
   /// Returns the inverse covariance matrix
-  const Matrix<>& get_C_inv() const {return my_C_inv;}
-  Matrix<>& get_C_inv() {return my_C_inv;}
-  /// Returns my_vector
-  const Vector<>& get_vector() const {return my_vector;}
-  Vector<>& get_vector(){return my_vector;}
+       const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;}
+       Vector<Size,Precision>& get_mu(){return my_mu;}
+       const Vector<Size,Precision>& get_mu() const {return my_mu;}
+       Vector<Size,Precision>& get_vector(){return my_vector;}
+       const Vector<Size,Precision>& get_vector() const {return my_vector;}
+       Decomposition<Size,Size,Precision>& get_decomposition(){return 
my_decomposition;}
+       const Decomposition<Size,Size,Precision>& get_decomposition() const 
{return my_decomposition;}
 
 
- private: 
-  int Size; 
-  Vector<> my_mu;
-  Vector<> my_vector;
-  Matrix<> my_C_inv;
+private:
+       Matrix<Size,Size,Precision> my_C_inv;
+       Vector<Size,Precision> my_vector;
+       Decomposition<Size,Size,Precision> my_decomposition;
+       Vector<Size,Precision> my_mu;
 
   // comment out to allow bitwise copying
   WLS( WLS& copyof );
   int operator = ( WLS& copyof );
-
-
-  //  To return SVD, have to create a point to an SVD<> instead of creating a 
temporary variable in compute()
-#if 0
-  SVD<Size>& get_svd(){return my_svd;}
-  const SVD<Size>& get_svd() const {return my_svd;}
-#endif
-
 };
 
-
-
-
-
-
-
-
-
-
-
-
-
 #ifndef TOON_NO_NAMESPACE
 }
 #endif