examples/calibrate3d/TooN/wls.h

00001 
00002 /*                       
00003          Copyright (C) 2005 Tom Drummond
00004 
00005      This library is free software; you can redistribute it and/or
00006      modify it under the terms of the GNU Lesser General Public
00007      License as published by the Free Software Foundation; either
00008      version 2.1 of the License, or (at your option) any later version.
00009 
00010      This library is distributed in the hope that it will be useful,
00011      but WITHOUT ANY WARRANTY; without even the implied warranty of
00012      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013      Lesser General Public License for more details.
00014 
00015      You should have received a copy of the GNU Lesser General Public
00016      License along with this library; if not, write to the Free Software
00017      Foundation, Inc.
00018      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020 #ifndef __WLS_H
00021 #define __WLS_H
00022 
00023 #include <TooN/TooN.h>
00024 #include <TooN/SVD.h>
00025 #include <TooN/helpers.h>
00026 
00027 #include <cassert>
00028 #include <cmath>
00029 
00030 #ifndef TOON_NO_NAMESPACE
00031 namespace TooN {
00032 #endif
00033 
00037 template <int Size=-1>
00038 class WLS {
00039 public:
00041   WLS(){clear();}
00043   WLS(double prior){clear(prior);}
00044 
00048   void clear(double prior=0){
00049     Identity(my_C_inv,prior);
00050     for(int i=0; i<Size; i++){
00051       my_vector[i]=0;
00052     }
00053   }
00054 
00058   void add_prior(double val){
00059     for(int i=0; i<Size; i++){
00060       my_C_inv(i,i)+=val;
00061     }
00062   }
00063   
00067   template<class Accessor>
00068   void add_prior(const FixedVector<Size,Accessor>& v){
00069     for(int i=0; i<Size; i++){
00070       my_C_inv(i,i)+=v[i];
00071     }
00072   }
00073 
00077   template<class Accessor>
00078   void add_prior(const FixedMatrix<Size,Size,Accessor>& m){
00079     my_C_inv+=m;
00080   }
00081 
00086   template<class Accessor>
00087   inline void add_df(double m, const FixedVector<Size,Accessor>& J, double weight = 1) {
00088     Vector<Size> Jw = J*weight;
00089     for(int i=0; i<Size; i++){
00090       for(int j=0; j<Size; j++){
00091         my_C_inv[i][j]+=J[i]*Jw[j];
00092       }
00093       my_vector[i]+=Jw[i]*m;
00094     }
00095   }
00096 
00102   template<int N, class Accessor1, class Accessor2, class Accessor3>
00103   inline void add_df(const FixedVector<N,Accessor1>& m,
00104                      const FixedMatrix<Size,N,Accessor2>& J,
00105                      const FixedMatrix<N,N,Accessor3>& invcov){
00106     my_C_inv += J * invcov * J.T();
00107     my_vector += J * invcov * m;
00108   }
00109 
00110   void compute(){
00111     my_svd.compute(my_C_inv);
00112     my_mu=my_svd.backsub(my_vector);
00113   }
00114 
00117   void operator += (const WLS& meas){
00118     my_vector+=meas.my_vector;
00119     my_C_inv += meas.my_C_inv;
00120   }
00121 
00123   Matrix<Size,Size,RowMajor>& get_C_inv() {return my_C_inv;}
00125   const Matrix<Size,Size,RowMajor>& get_C_inv() const {return my_C_inv;}
00126   Vector<Size>& get_mu(){return my_mu;}
00127   const Vector<Size>& get_mu() const {return my_mu;}
00128   Vector<Size>& get_vector(){return my_vector;}
00129   const Vector<Size>& get_vector() const {return my_vector;}
00130   SVD<Size>& get_svd(){return my_svd;}
00131   const SVD<Size>& get_svd() const {return my_svd;}
00132 
00133 private:
00134   Vector<Size> my_mu;
00135   Matrix<Size,Size,RowMajor> my_C_inv;
00136   Vector<Size> my_vector;
00137   SVD<Size,Size> my_svd;
00138 
00139   // comment out to allow bitwise copying
00140   WLS( WLS& copyof );
00141   int operator = ( WLS& copyof );
00142 };
00143 
00144 
00145 
00146 
00147 
00148 
00149 
00150 //  syg21: Dynamic WLS
00151 
00155 template<> 
00156 class WLS<-1> {
00157 public:
00159    //WLS(){clear();} 
00160   WLS(unsigned int s) 
00161     : Size(s), my_mu(Size), my_vector(Size), my_C_inv(Size, Size)
00162     { 
00163       clear();
00164     } 
00165      
00166     void Identity(Matrix<> &M, double d)
00167     {
00168       for(int r=0; r<M.num_rows(); r++)
00169         {
00170           for(int c=0; c<M.num_cols(); c++)
00171             {
00172               M[r][c] = 0.0;
00173             }
00174           M[r][r] = 1.0;
00175         }
00176     }
00177 
00181   void clear(double prior=0){
00182     Identity(my_C_inv,prior);
00183     for(int i=0; i<Size; i++){
00184       my_vector[i]=0;
00185     }
00186   }
00187 
00191   void add_prior(double val){
00192       for(int i=0; i<Size; i++){
00193       my_C_inv(i,i)+=val;
00194     }
00195   }
00196   
00200   template<int VSize, class Accessor>
00201   void add_prior(const FixedVector<VSize,Accessor>& v){
00202     assert(VSize==Size);
00203     for(int i=0; i<VSize; i++){
00204       my_C_inv(i,i)+=v[i];
00205     }
00206   }
00210   template<int MSize, class Accessor>
00211   void add_prior(const FixedMatrix<MSize,MSize,Accessor>& m){
00212     my_C_inv+=m;
00213   }
00214 
00219   template<int VSize, class Accessor>
00220   inline void add_df(double m, const FixedVector<VSize,Accessor>& J, double weight = 1) {
00221     assert(VSize==Size);
00222     Vector<VSize> Jw = J*weight;
00223     for(int i=0; i<VSize; i++){
00224       for(int j=0; j<VSize; j++){
00225         my_C_inv[i][j]+=J[i]*Jw[j];
00226       }
00227       my_vector[i]+=Jw[i]*m;
00228     }
00229   }
00230 
00235   template<class Accessor>
00236     inline void add_df(double m, const DynamicVector<Accessor>& J, double weight = 1) {
00237     Vector<> Jw(Size);
00238     Jw = J * weight;
00239     for(int i=0; i<Size; i++){
00240       for(int j=0; j<Size; j++){
00241         my_C_inv[i][j]+=J[i]*Jw[j];
00242       }
00243       my_vector[i]+=Jw[i]*m;
00244     }
00245   }
00246 
00247 
00248   void compute(){
00249     //  create SVD
00250     SVD<> my_svd(my_C_inv);
00251     //my_svd.compute(my_C_inv);
00252     my_mu=my_svd.backsub(my_vector);
00253   }
00254 
00256   const Vector<>& get_mu() const {return my_mu;}
00257   Vector<>& get_mu() {return my_mu;}
00259   const Matrix<>& get_C_inv() const {return my_C_inv;}
00260   Matrix<>& get_C_inv() {return my_C_inv;}
00262   const Vector<>& get_vector() const {return my_vector;}
00263   Vector<>& get_vector(){return my_vector;}
00264 
00265 
00266  private: 
00267   int Size; 
00268   Vector<> my_mu;
00269   Vector<> my_vector;
00270   Matrix<> my_C_inv;
00271 
00272   // comment out to allow bitwise copying
00273   WLS( WLS& copyof );
00274   int operator = ( WLS& copyof );
00275 
00276 
00277   //  To return SVD, have to create a point to an SVD<> instead of creating a temporary variable in compute()
00278 #if 0
00279   SVD<Size>& get_svd(){return my_svd;}
00280   const SVD<Size>& get_svd() const {return my_svd;}
00281 #endif
00282 
00283 };
00284 
00285 
00286 
00287 
00288 
00289 
00290 
00291 
00292 
00293 
00294 
00295 
00296 
00297 #ifndef TOON_NO_NAMESPACE
00298 }
00299 #endif
00300 
00301 #endif

Generated on Fri Feb 22 18:26:55 2008 for QVision by  doxygen 1.5.3