TooN: wls.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

wls.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
    00004 //
    00005 // This file is part of the TooN Library.  This library is free
    00006 // software; you can redistribute it and/or modify it under the
    00007 // terms of the GNU General Public License as published by the
    00008 // Free Software Foundation; either version 2, or (at your option)
    00009 // any later version.
    00010
    00011 // This library is distributed in the hope that it will be useful,
    00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
    00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    00014 // GNU General Public License for more details.
    00015
    00016 // You should have received a copy of the GNU General Public License along
    00017 // with this library; see the file COPYING.  If not, write to the Free
    00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
    00019 // USA.
    00020
    00021 // As a special exception, you may use this file as part of a free software
    00022 // library without restriction.  Specifically, if other files instantiate
    00023 // templates or use macros or inline functions from this file, or you compile
    00024 // this file and link it with other files to produce an executable, this
    00025 // file does not by itself cause the resulting executable to be covered by
    00026 // the GNU General Public License.  This exception does not however
    00027 // invalidate any other reasons why the executable file might be covered by
    00028 // the GNU General Public License.
    00029
    00030 #ifndef TOON_INCLUDE_WLS_H
    00031 #define TOON_INCLUDE_WLS_H
    00032
    00033 #include <TooN/TooN.h>
    00034 #include <TooN/Cholesky.h>
    00035 #include <TooN/helpers.h>
    00036
    00037 #include <cmath>
    00038
    00039 namespace TooN {
    00040
    00041 /// Performs Gauss-Newton weighted least squares computation.
    00042 /// @param Size The number of dimensions in the system
    00043 /// @param Precision The numerical precision used (double, float etc)
    00044 /// @param Decomposition The class used to invert the inverse Covariance matrix (must have one integer size and one typename precision template arguments) this is Cholesky by default, but could also be SQSVD
    00045 /// @ingroup gEquations
    00046 template <int Size=Dynamic, class Precision=DefaultPrecision,
    00047           template<int DecompSize, class DecompPrecision> class Decomposition = Cholesky>
    00048 class WLS {
    00049 public:
    00050
    00051     /// Default constructor or construct with the number of dimensions for the Dynamic case
    00052     WLS(int size=0) :
    00053         my_C_inv(size,size),
    00054         my_vector(size),
    00055         my_decomposition(size),
    00056         my_mu(size)
    00057     {
    00058         clear();
    00059     }
    00060
    00061     /// Clear all the measurements and apply a constant regularisation term.
    00062     void clear(){
    00063         my_C_inv = Zeros;
    00064         my_vector = Zeros;
    00065     }
    00066
    00067     /// Applies a constant regularisation term.
    00068     /// Equates to a prior that says all the parameters are zero with \f$\sigma^2 = \frac{1}{\text{val}}\f$.
    00069     /// @param val The strength of the prior
    00070     void add_prior(Precision val){
    00071         for(int i=0; i<my_C_inv.num_rows(); i++){
    00072             my_C_inv(i,i)+=val;
    00073         }
    00074     }
    00075
    00076     /// Applies a regularisation term with a different strength for each parameter value.
    00077     /// Equates to a prior that says all the parameters are zero with \f$\sigma_i^2 = \frac{1}{\text{v}_i}\f$.
    00078     /// @param v The vector of priors
    00079     template<class B2>
    00080     void add_prior(const Vector<Size,Precision,B2>& v){
    00081         SizeMismatch<Size,Size>::test(my_C_inv.num_rows(), v.size());
    00082         for(int i=0; i<my_C_inv.num_rows(); i++){
    00083             my_C_inv(i,i)+=v[i];
    00084         }
    00085     }
    00086
    00087     /// Applies a whole-matrix regularisation term.
    00088     /// This is the same as adding the \f$m\f$ to the inverse covariance matrix.
    00089     /// @param m The inverse covariance matrix to add
    00090     template<class B2>
    00091     void add_prior(const Matrix<Size,Size,Precision,B2>& m){
    00092         my_C_inv+=m;
    00093     }
    00094
    00095     /// Add a single measurement
    00096     /// @param m The value of the measurement
    00097     /// @param J The Jacobian for the measurement \f$\frac{\partial\text{m}}{\partial\text{param}_i}\f$
    00098     /// @param weight The inverse variance of the measurement (default = 1)
    00099     template<class B2>
    00100     inline void add_mJ(Precision m, const Vector<Size, Precision, B2>& J, Precision weight = 1) {
    00101
    00102         //Upper right triangle only, for speed
    00103         for(int r=0; r < my_C_inv.num_rows(); r++)
    00104         {
    00105             double Jw = weight * J[r];
    00106             my_vector[r] += m * Jw;
    00107             for(int c=r; c < my_C_inv.num_rows(); c++)
    00108                 my_C_inv[r][c] += Jw * J[c];
    00109         }
    00110     }
    00111
    00112     /// Add multiple measurements at once (much more efficiently)
    00113     /// @param m The measurements to add
    00114     /// @param J The Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00115     /// @param invcov The inverse covariance of the measurement values
    00116     template<int N, class B1, class B2, class B3>
    00117     inline void add_mJ(const Vector<N,Precision,B1>& m,
    00118                        const Matrix<Size,N,Precision,B2>& J,
    00119                        const Matrix<N,N,Precision,B3>& invcov){
    00120         const Matrix<Size,N,Precision> temp =  J * invcov;
    00121         my_C_inv += temp * J.T();
    00122         my_vector += temp * m;
    00123     }
    00124
    00125     /// Add multiple measurements at once (much more efficiently)
    00126     /// @param m The measurements to add
    00127     /// @param J The Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00128     /// @param invcov The inverse covariance of the measurement values
    00129     template<int N, class B1, class B2, class B3>
    00130     inline void add_mJ_rows(const Vector<N,Precision,B1>& m,
    00131                        const Matrix<N,Size,Precision,B2>& J,
    00132                        const Matrix<N,N,Precision,B3>& invcov){
    00133         const Matrix<Size,N,Precision> temp =  J.T() * invcov;
    00134         my_C_inv += temp * J;
    00135         my_vector += temp * m;
    00136     }
    00137
    00138     /// Add a single measurement at once with a sparse Jacobian (much, much more efficiently)
    00139     /// @param m The measurements to add
    00140     /// @param J1 The first block of the Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00141     /// @param index1 starting index for the first block
    00142     /// @param invcov The inverse covariance of the measurement values
    00143     template<int N, typename B1>
    00144     inline void add_sparse_mJ(const Precision m,
    00145                        const Vector<N,Precision,B1>& J1, const int index1,
    00146                        const Precision weight = 1){
    00147         //Upper right triangle only, for speed
    00148         for(int r=0; r < J1.size(); r++)
    00149         {
    00150             double Jw = weight * J1[r];
    00151             my_vector[r+index1] += m * Jw;
    00152             for(int c = r; c < J1.size(); c++)
    00153                 my_C_inv[r+index1][c+index1] += Jw * J1[c];
    00154         }
    00155     }
    00156
    00157     /// Add multiple measurements at once with a sparse Jacobian (much, much more efficiently)
    00158     /// @param m The measurements to add
    00159     /// @param J1 The first block of the Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00160     /// @param index1 starting index for the first block
    00161     /// @param invcov The inverse covariance of the measurement values
    00162     template<int N, int S1, class P1, class P2, class P3, class B1, class B2, class B3>
    00163     inline void add_sparse_mJ_rows(const Vector<N,P1,B1>& m,
    00164                        const Matrix<N,S1,P2,B2>& J1, const int index1,
    00165                        const Matrix<N,N,P3,B3>& invcov){
    00166         const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
    00167         const int size1 = J1.num_cols();
    00168         my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
    00169         my_vector.slice(index1, size1) += temp1 * m;
    00170     }
    00171
    00172     /// Add multiple measurements at once with a sparse Jacobian (much, much more efficiently)
    00173     /// @param m The measurements to add
    00174     /// @param J1 The first block of the Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00175     /// @param index1 starting index for the first block
    00176     /// @param J2 The second block of the Jacobian matrix \f$\frac{\partial\text{m}_i}{\partial\text{param}_j}\f$
    00177     /// @param index2 starting index for the second block
    00178     /// @param invcov The inverse covariance of the measurement values
    00179     template<int N, int S1, int S2, class B1, class B2, class B3, class B4>
    00180     inline void add_sparse_mJ_rows(const Vector<N,Precision,B1>& m,
    00181                        const Matrix<N,S1,Precision,B2>& J1, const int index1,
    00182                        const Matrix<N,S2,Precision,B3>& J2, const int index2,
    00183                        const Matrix<N,N,Precision,B4>& invcov){
    00184         const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
    00185         const Matrix<S2,N,Precision> temp2 = J2.T() * invcov;
    00186         const Matrix<S1,S2,Precision> mixed = temp1 * J2;
    00187         const int size1 = J1.num_cols();
    00188         const int size2 = J2.num_cols();
    00189         my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
    00190         my_C_inv.slice(index2, index2, size2, size2) += temp2 * J2;
    00191         my_C_inv.slice(index1, index2, size1, size2) += mixed;
    00192         my_C_inv.slice(index2, index1, size2, size1) += mixed.T();
    00193         my_vector.slice(index1, size1) += temp1 * m;
    00194         my_vector.slice(index2, size2) += temp2 * m;
    00195     }
    00196
    00197     /// Process all the measurements and compute the weighted least squares set of parameter values
    00198     /// stores the result internally which can then be accessed by calling get_mu()
    00199     void compute(){
    00200
    00201         //Copy the upper right triangle to the empty lower-left.
    00202         for(int r=1; r < my_C_inv.num_rows(); r++)
    00203             for(int c=0; c < r; c++)
    00204                 my_C_inv[r][c] = my_C_inv[c][r];
    00205
    00206         my_decomposition.compute(my_C_inv);
    00207         my_mu=my_decomposition.backsub(my_vector);
    00208     }
    00209
    00210     /// Combine measurements from two WLS systems
    00211     /// @param meas The measurements to combine with
    00212     void operator += (const WLS& meas){
    00213         my_vector+=meas.my_vector;
    00214         my_C_inv += meas.my_C_inv;
    00215     }
    00216
    00217     /// Returns the inverse covariance matrix
    00218     Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;}
    00219     /// Returns the inverse covariance matrix
    00220     const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;}
    00221     Vector<Size,Precision>& get_mu(){return my_mu;}  ///<Returns the update. With no prior, this is the result of \f$J^\dagger e\f$.
    00222     const Vector<Size,Precision>& get_mu() const {return my_mu;} ///<Returns the update. With no prior, this is the result of \f$J^\dagger e\f$.
    00223     Vector<Size,Precision>& get_vector(){return my_vector;} ///<Returns the  vector \f$J^{\mathsf T} e\f$
    00224     const Vector<Size,Precision>& get_vector() const {return my_vector;} ///<Returns the  vector \f$J^{\mathsf T} e\f$
    00225     Decomposition<Size,Precision>& get_decomposition(){return my_decomposition;} ///< Return the decomposition object used to compute \f$(J^{\mathsf T}  J + P)^{-1}\f$
    00226     const Decomposition<Size,Precision>& get_decomposition() const {return my_decomposition;} ///< Return the decomposition object used to compute \f$(J^{\mathsf T}  J + P)^{-1}\f$
    00227
    00228
    00229 private:
    00230     Matrix<Size,Size,Precision> my_C_inv;
    00231     Vector<Size,Precision> my_vector;
    00232     Decomposition<Size,Precision> my_decomposition;
    00233     Vector<Size,Precision> my_mu;
    00234
    00235     // comment out to allow bitwise copying
    00236     // WLS( WLS& copyof );
    00237     // int operator = ( WLS& copyof );
    00238 };
    00239
    00240 }
    00241
    00242 #endif