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=double, 00047 template<int Size, class Precision> 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 Matrix<Size,N,Precision> temp = J * invcov; 00121 my_C_inv += temp * J.T(); 00122 my_vector += temp * m; 00123 } 00124 00125 00126 /// Process all the measurements and compute the weighted least squares set of parameter values 00127 /// stores the result internally which can then be accessed by calling get_mu() 00128 void compute(){ 00129 00130 //Copy the upper right triangle to the empty lower-left. 00131 for(int r=1; r < my_C_inv.num_rows(); r++) 00132 for(int c=0; c < r; c++) 00133 my_C_inv[r][c] = my_C_inv[c][r]; 00134 00135 my_decomposition.compute(my_C_inv); 00136 my_mu=my_decomposition.backsub(my_vector); 00137 } 00138 00139 /// Combine measurements from two WLS systems 00140 /// @param meas The measurements to combine with 00141 void operator += (const WLS& meas){ 00142 my_vector+=meas.my_vector; 00143 my_C_inv += meas.my_C_inv; 00144 } 00145 00146 /// Returns the inverse covariance matrix 00147 Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;} 00148 /// Returns the inverse covariance matrix 00149 const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;} 00150 Vector<Size,Precision>& get_mu(){return my_mu;} ///<Returns the update. With no prior, this is the result of \f$J^\dagger e\f$. 00151 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$. 00152 Vector<Size,Precision>& get_vector(){return my_vector;} ///<Returns the vector \f$J^{\mathsf T} e\f$ 00153 const Vector<Size,Precision>& get_vector() const {return my_vector;} ///<Returns the vector \f$J^{\mathsf T} e\f$ 00154 Decomposition<Size,Precision>& get_decomposition(){return my_decomposition;} ///< Return the decomposition object used to compute \f$(J^{\mathsf T} J + P)^{-1}\f$ 00155 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$ 00156 00157 00158 private: 00159 Matrix<Size,Size,Precision> my_C_inv, my_C_inv2; 00160 Vector<Size,Precision> my_vector; 00161 Decomposition<Size,Precision> my_decomposition; 00162 Vector<Size,Precision> my_mu; 00163 00164 // comment out to allow bitwise copying 00165 WLS( WLS& copyof ); 00166 int operator = ( WLS& copyof ); 00167 }; 00168 00169 } 00170 00171 #endif