00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
00156 WLS( WLS& copyof );
00157 int operator = ( WLS& copyof );
00158 };
00159
00160 }
00161
00162 #endif