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 
00046 template <int Size=Dynamic, class Precision=double,
00047           template<int Size, class Precision> class Decomposition = Cholesky>
00048 class WLS {
00049 public:
00050 
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 
00064     void clear(){
00065         my_C_inv = Zeros;
00066         my_vector = Zeros;
00067     }
00068 
00072     void add_prior(Precision val){
00073         for(int i=0; i<my_C_inv.num_rows(); i++){
00074             my_C_inv(i,i)+=val;
00075         }
00076     }
00077   
00081     template<class B2>
00082     void add_prior(const Vector<Size,Precision,B2>& v){
00083         SizeMismatch<Size,Size>::test(my_C_inv.num_rows(), v.size());
00084         for(int i=0; i<my_C_inv.num_rows(); i++){
00085             my_C_inv(i,i)+=v[i];
00086         }
00087     }
00088 
00092     template<class B2>
00093     void add_prior(const Matrix<Size,Size,Precision,B2>& m){
00094         my_C_inv+=m;
00095     }
00096 
00101     template<class B2>
00102     inline void add_mJ(Precision m, const Vector<Size, Precision, B2>& J, Precision weight = 1) {
00103         Vector<Size,Precision> Jw = J*weight;
00104         my_C_inv += Jw.as_col() * J.as_row();
00105         my_vector+= m*Jw;
00106     }
00107 
00113     template<int N, class B1, class B2, class B3>
00114     inline void add_mJ(const Vector<N,Precision,B1>& m,
00115                        const Matrix<Size,N,Precision,B2>& J,
00116                        const Matrix<N,N,Precision,B3>& invcov){
00117         Matrix<Size,N,Precision> temp =  J * invcov;
00118         my_C_inv += temp * J.T();
00119         my_vector += temp * m;
00120     }
00121 
00122 
00125     void compute(){
00126         my_decomposition.compute(my_C_inv);
00127         my_mu=my_decomposition.backsub(my_vector);
00128     }
00129 
00132     void operator += (const WLS& meas){
00133         my_vector+=meas.my_vector;
00134         my_C_inv += meas.my_C_inv;
00135     }
00136 
00138     Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;}
00140     const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;}
00141     Vector<Size,Precision>& get_mu(){return my_mu;}
00142     const Vector<Size,Precision>& get_mu() const {return my_mu;}
00143     Vector<Size,Precision>& get_vector(){return my_vector;}
00144     const Vector<Size,Precision>& get_vector() const {return my_vector;}
00145     Decomposition<Size,Precision>& get_decomposition(){return my_decomposition;}
00146     const Decomposition<Size,Precision>& get_decomposition() const {return my_decomposition;}
00147 
00148 
00149 private:
00150     Matrix<Size,Size,Precision> my_C_inv;
00151     Vector<Size,Precision> my_vector;
00152     Decomposition<Size,Precision> my_decomposition;
00153     Vector<Size,Precision> my_mu;
00154 
00155     // comment out to allow bitwise copying
00156     WLS( WLS& copyof );
00157     int operator = ( WLS& copyof );
00158 };
00159 
00160 }
00161 
00162 #endif

Generated on Thu May 7 20:28:41 2009 for TooN by  doxygen 1.5.3