examples/calibrate3d/TooN/wls_cholesky.h

00001 /*                       
00002         Copyright (C) 2005 Tom Drummond
00003 
00004         This library is free software; you can redistribute it and/or
00005         modify it under the terms of the GNU Lesser General Public
00006         License as published by the Free Software Foundation; either
00007         version 2.1 of the License, or (at your option) any later version.
00008 
00009         This library is distributed in the hope that it will be useful,
00010         but WITHOUT ANY WARRANTY; without even the implied warranty of
00011         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012         Lesser General Public License for more details.
00013 
00014         You should have received a copy of the GNU Lesser General Public
00015         License along with this library; if not, write to the Free Software
00016         Foundation, Inc.
00017      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00018     51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00019 */
00020 //-*- c++ -*-
00021 // A WLS class using Cholesky decomposition and sparse JtJ
00022 // Also stores the sum squared error and can compute the residual
00023 
00024 #ifndef __WLS_CHOLESKY_H
00025 #define __WLS_CHOLESKY_H
00026 
00027 #include <TooN/TooN.h>
00028 #include <cassert>
00029 #include <cmath>
00030 
00031 namespace TooN {
00032 
00038 template <int Size = -1>
00039 class WLSCholesky {
00040 public:
00042   WLSCholesky(){clear();}
00044   WLSCholesky(double prior){clear(prior);}
00047   WLSCholesky(const WLSCholesky &w) {
00048     my_C_inv=w.my_C_inv;
00049     my_err=w.my_err;
00050     my_extra=w.my_extra;
00051     my_vector=w.my_vector;
00052   }
00053 
00057   void clear(double prior=0){
00058     Identity(my_C_inv,prior);
00059     for(int i=0; i<Size; i++){
00060       my_vector[i]=0;
00061     }
00062     my_err=0;
00063     my_extra=0;
00064   }
00065 
00069   void add_prior(double val){
00070     for(int i=0; i<Size; i++){
00071       my_C_inv(i,i)+=val;
00072     }
00073   }
00074   
00079   template<class Accessor>
00080   void add_prior(double val, const FixedVector<Size,Accessor>& pos){
00081     for(int i=0; i<Size; i++){
00082       my_C_inv(i,i)+=val;
00083     }
00084     my_vector+=pos*val;
00085     my_err+=val*(pos*pos);
00086   }
00087 
00091   template<class Accessor>
00092   void add_prior(const FixedVector<Size,Accessor>& v){
00093     for(int i=0; i<Size; i++){
00094       my_C_inv(i,i)+=v[i];
00095     }
00096   }
00097 
00101   template<class Accessor>
00102   void add_prior(const FixedMatrix<Size,Size,Accessor>& m){
00103     my_C_inv+=m;
00104   }
00105 
00110   template<class Accessor>
00111   inline void add_df(double m, const FixedVector<Size,Accessor>& J, double weight = 1) {
00112     Vector<Size> Jw = J*weight;
00113     for(int i=0; i<Size; i++){
00114       for(int j=i; j<Size; j++){
00115         my_C_inv[i][j]+=J[i]*Jw[j];
00116       }
00117       my_vector[i]+=Jw[i]*m;
00118     }
00119     my_err+=m*weight*m;
00120   }
00121 
00127   template<int N, class Accessor1, class Accessor2, class Accessor3>
00128   inline void add_df(const FixedVector<N,Accessor1>& m,
00129                      const FixedMatrix<Size,N,Accessor2>& J,
00130                      const FixedMatrix<N,N,Accessor3>& invcov){
00131     my_C_inv += J * invcov * J.T();  // FIXME make me faster!
00132     Vector<N> temp(invcov*m);
00133     my_vector += J * temp;
00134     my_err += m*temp;
00135   }
00136 
00137   
00138   void compute(){
00139     // Homegrown Cholesky
00140     Matrix<Size> L;
00141     for(int i=0;i<Size;i++) {
00142       double a=my_C_inv[i][i];
00143       for(int k=0;k<i;k++) a-=L[k][i]*L[k][i];
00144       L[i][i]=sqrt(a);
00145       for(int j=i;j<Size;j++) {
00146         a=my_C_inv[i][j];
00147         for(int k=0;k<i;k++) a-=L[k][j]*L[k][i];
00148         L[i][j]=a/L[i][i];
00149       }
00150     }
00151     Vector<Size> y;
00152     for(int i=0;i<Size;i++) {
00153       double a=my_vector[i];
00154       for(int j=0;j<i;j++) a-=L[j][i]*y[j];
00155       y[i]=a/L[i][i];
00156     }
00157     for(int i=Size-1;i>-1;i--) {
00158       double a=y[i];
00159       for(int j=i+1;j<Size;j++) a-=L[i][j]*my_mu[j];
00160       my_mu[i]=a/L[i][i];
00161     } 
00162   }
00163 
00166   void operator += (const WLSCholesky& meas){
00167     my_vector+=meas.my_vector;
00168     my_C_inv+=meas.my_C_inv;
00169     my_err+=meas.my_err;
00170     my_extra+=meas.my_extra;
00171   }
00172 
00175   void operator = (const WLSCholesky &w) {
00176     my_C_inv=w.my_C_inv;
00177     my_err=w.my_err;
00178     my_vector=w.my_vector;
00179     my_extra=w.my_extra;
00180   }
00181 
00183   Matrix<Size,Size,RowMajor>& get_C_inv() {return my_C_inv;}
00185   const Matrix<Size,Size,RowMajor>& get_C_inv() const {return my_C_inv;}
00186   Vector<Size>& get_mu(){return my_mu;}
00187   const Vector<Size>& get_mu() const {return my_mu;}
00188   Vector<Size>& get_vector(){return my_vector;}
00189   const Vector<Size>& get_vector() const {return my_vector;}
00190   double get_residual(){
00191     Vector<Size> temp;
00192     Zero(temp);
00193     for(int ii=0;ii<Size;ii++) {
00194       temp[ii]+=my_C_inv[ii][ii]*my_mu[ii];
00195       for(int jj=ii+1;jj<Size;jj++) {
00196         temp[ii]+=my_C_inv[ii][jj]*my_mu[jj];
00197         temp[jj]+=my_C_inv[ii][jj]*my_mu[ii];
00198       }
00199     }
00200     return my_err-my_mu*temp;
00201   }
00202 
00203   inline void add_extra(double e) {my_extra+=e;}
00204   inline double get_extra() {return my_extra;}
00205 
00206 private:
00207   Vector<Size> my_mu;
00208   Matrix<Size,Size,RowMajor> my_C_inv;
00209   Vector<Size> my_vector;
00210   double my_err;    // error before optimisation
00211   double my_extra;  // extra residual error 
00212 };
00213 
00220 template <>
00221 class WLSCholesky<-1> {
00222 public:
00224   WLSCholesky(){clear();}
00226   WLSCholesky(double prior){clear(prior);}
00228   WLSCholesky(int Size, double prior = 0.0){
00229     resize(Size);
00230     clear(prior);
00231   }
00234   WLSCholesky(const WLSCholesky &w) {
00235     resize(w.size());
00236     my_C_inv=w.my_C_inv;
00237     my_err=w.my_err;
00238     my_extra=w.my_extra;
00239     my_vector=w.my_vector;
00240   }
00241 
00243   void resize(int N){
00244     my_C_inv.resize(N,N);
00245     my_vector.resize(N);
00246     my_mu.resize(N);
00247   }
00248 
00250   int size(void) const throw() {
00251     return my_vector.size();
00252   }
00253 
00257   void clear(double prior=0){
00258     Identity(my_C_inv,prior);
00259     for(int i=0; i<size(); i++){
00260       my_vector[i]=0;
00261     }
00262     my_err=0;
00263     my_extra=0;
00264   }
00265 
00269   void add_prior(double val){
00270     for(int i=0; i<size(); i++){
00271       my_C_inv(i,i)+=val;
00272     }
00273   }
00274 
00279   template<class Accessor, int N>
00280   void add_prior(double val, const FixedVector<N,Accessor>& pos){
00281     assert(N == size());
00282     for(int i=0; i<N; i++){
00283       my_C_inv(i,i)+=val;
00284     }
00285     my_vector+=pos*val;
00286     my_err+=val*(pos*pos);
00287   }
00288 
00292   template<class Accessor, int N>
00293   void add_prior(const FixedVector<N,Accessor>& v){
00294     assert(N == size());
00295     for(int i=0; i<N; i++){
00296       my_C_inv(i,i)+=v[i];
00297     }
00298   }
00299 
00303   template<class Accessor, int N>
00304   void add_prior(const FixedMatrix<N,N,Accessor>& m){
00305     assert(N == size());
00306     my_C_inv+=m;
00307   }
00308 
00313   template<class Accessor, int N>
00314   inline void add_df(double m, const FixedVector<N,Accessor>& J, double weight = 1) {
00315     assert(N == size());
00316     Vector<N> Jw = J*weight;
00317     for(int i=0; i<N; i++){
00318       for(int j=i; j<N; j++){
00319         my_C_inv[i][j]+=J[i]*Jw[j];
00320       }
00321       my_vector[i]+=Jw[i]*m;
00322     }
00323     my_err+=m*weight*m;
00324   }
00325 
00330   template<class Accessor>
00331   inline void add_df(double m, const DynamicVector<Accessor>& J, double weight = 1) {
00332     assert(J.size() == size());
00333     const int Size =size();
00334     Vector<-1> Jw = J*weight;
00335     for(int i=0; i<Size; i++){
00336       for(int j=i; j<Size; j++){
00337         my_C_inv[i][j]+=J[i]*Jw[j];
00338       }
00339       my_vector[i]+=Jw[i]*m;
00340     }
00341     my_err+=m*weight*m;
00342   }
00343 
00349   template<int N, class Accessor1, class Accessor2, class Accessor3, int M>
00350   inline void add_df(const FixedVector<N,Accessor1>& m,
00351                      const FixedMatrix<M,N,Accessor2>& J,
00352                      const FixedMatrix<N,N,Accessor3>& invcov){
00353     assert(M == size());
00354     my_C_inv += J * invcov * J.T();  // FIXME make me faster!
00355     Vector<N> temp(invcov*m);
00356     my_vector += J * temp;
00357     my_err += m*temp;
00358   }
00359 
00365   template<class Accessor1, class Accessor2, class Accessor3>
00366   inline void add_df(const DynamicVector<Accessor1>& m,
00367                      const DynamicMatrix<Accessor2>& J,
00368                      const DynamicMatrix<Accessor3>& invcov){
00369     // FIXME add some meaningful asserts here
00370     my_C_inv += J * invcov * J.T();  // FIXME make me faster!
00371     Vector<-1> temp(invcov*m);
00372     my_vector += J * temp;
00373     my_err += m*temp;
00374   }
00375 
00376   void compute(){
00377     const int Size = size();
00378     // Homegrown Cholesky
00379     Matrix<-1> L(Size, Size);
00380     my_mu.resize(Size);
00381     for(int i=0;i<Size;i++) {
00382       double a=my_C_inv[i][i];
00383       for(int k=0;k<i;k++) a-=L[k][i]*L[k][i];
00384       L[i][i]=sqrt(a);
00385       for(int j=i;j<Size;j++) {
00386         a=my_C_inv[i][j];
00387         for(int k=0;k<i;k++) a-=L[k][j]*L[k][i];
00388         L[i][j]=a/L[i][i];
00389       }
00390     }
00391     Vector<-1> y(Size);
00392     for(int i=0;i<Size;i++) {
00393       double a=my_vector[i];
00394       for(int j=0;j<i;j++) a-=L[j][i]*y[j];
00395       y[i]=a/L[i][i];
00396     }
00397     for(int i=Size-1;i>-1;i--) {
00398       double a=y[i];
00399       for(int j=i+1;j<Size;j++) a-=L[i][j]*my_mu[j];
00400       my_mu[i]=a/L[i][i];
00401     }
00402   }
00403 
00406   void operator += (const WLSCholesky& meas){
00407     assert(size() == meas.size());
00408     my_vector+=meas.my_vector;
00409     my_C_inv+=meas.my_C_inv;
00410     my_err+=meas.my_err;
00411     my_extra+=meas.my_extra;
00412   }
00413 
00416   void operator = (const WLSCholesky &w) {
00417     resize(w.size());
00418     my_C_inv=w.my_C_inv;
00419     my_err=w.my_err;
00420     my_vector=w.my_vector;
00421     my_extra=w.my_extra;
00422   }
00423 
00425   Matrix<-1,-1,RowMajor>& get_C_inv() {return my_C_inv;}
00427   const Matrix<-1,-1,RowMajor>& get_C_inv() const {return my_C_inv;}
00428   Vector<-1>& get_mu(){return my_mu;}
00429   const Vector<-1>& get_mu() const {return my_mu;}
00430   Vector<-1>& get_vector(){return my_vector;}
00431   const Vector<-1>& get_vector() const {return my_vector;}
00432   double get_residual(){
00433    const int Size = size();
00434     Vector<-1> temp(Size);
00435     Zero(temp);
00436     for(int ii=0;ii<Size;ii++) {
00437       temp[ii]+=my_C_inv[ii][ii]*my_mu[ii];
00438       for(int jj=ii+1;jj<Size;jj++) {
00439         temp[ii]+=my_C_inv[ii][jj]*my_mu[jj];
00440         temp[jj]+=my_C_inv[ii][jj]*my_mu[ii];
00441       }
00442     }
00443     return my_err-my_mu*temp;
00444   }
00445 
00446   inline void add_extra(double e) {my_extra+=e;}
00447   inline double get_extra() {return my_extra;}
00448 
00449 private:
00450   Vector<-1> my_mu;
00451   Matrix<-1,-1,RowMajor> my_C_inv;
00452   Vector<-1> my_vector;
00453   double my_err;    // error before optimisation
00454   double my_extra;  // extra residual error
00455 };
00456 
00457 }
00458 
00459 #endif

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