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
00041
00042
00043
00044
00045
00046 template <int Size=Dynamic, class Precision=double,
00047 template<int Size, class Precision> class Decomposition = Cholesky>
00048 class WLS {
00049 public:
00050
00051
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
00062 void clear(){
00063 my_C_inv = Zeros;
00064 my_vector = Zeros;
00065 }
00066
00067
00068
00069
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
00077
00078
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
00088
00089
00090 template<class B2>
00091 void add_prior(const Matrix<Size,Size,Precision,B2>& m){
00092 my_C_inv+=m;
00093 }
00094
00095
00096
00097
00098
00099 template<class B2>
00100 inline void add_mJ(Precision m, const Vector<Size, Precision, B2>& J, Precision weight = 1) {
00101
00102
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
00113
00114
00115
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 const Matrix<Size,N,Precision> temp = J * invcov;
00121 my_C_inv += temp * J.T();
00122 my_vector += temp * m;
00123 }
00124
00125
00126
00127
00128
00129 template<int N, class B1, class B2, class B3>
00130 inline void add_mJ_rows(const Vector<N,Precision,B1>& m,
00131 const Matrix<N,Size,Precision,B2>& J,
00132 const Matrix<N,N,Precision,B3>& invcov){
00133 const Matrix<Size,N,Precision> temp = J.T() * invcov;
00134 my_C_inv += temp * J;
00135 my_vector += temp * m;
00136 }
00137
00138
00139
00140
00141
00142
00143 template<int N, typename B1>
00144 inline void add_sparse_mJ(const Precision m,
00145 const Vector<N,Precision,B1>& J1, const int index1,
00146 const Precision weight){
00147
00148 for(int r=0; r < J1.size(); r++)
00149 {
00150 double Jw = weight * J1[r];
00151 my_vector[r+index1] += m * Jw;
00152 for(int c = r; c < J1.size(); c++)
00153 my_C_inv[r+index1][c+index1] += Jw * J1[c];
00154 }
00155 }
00156
00157
00158
00159
00160
00161
00162 template<int N, int S1, class P1, class P2, class P3, class B1, class B2, class B3>
00163 inline void add_sparse_mJ_rows(const Vector<N,P1,B1>& m,
00164 const Matrix<N,S1,P2,B2>& J1, const int index1,
00165 const Matrix<N,N,P3,B3>& invcov){
00166 const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
00167 const int size1 = J1.num_cols();
00168 my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
00169 my_vector.slice(index1, size1) += temp1 * m;
00170 }
00171
00172
00173
00174
00175
00176
00177
00178
00179 template<int N, int S1, int S2, class B1, class B2, class B3, class B4>
00180 inline void add_sparse_mJ_rows(const Vector<N,Precision,B1>& m,
00181 const Matrix<N,S1,Precision,B2>& J1, const int index1,
00182 const Matrix<N,S2,Precision,B3>& J2, const int index2,
00183 const Matrix<N,N,Precision,B4>& invcov){
00184 const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
00185 const Matrix<S2,N,Precision> temp2 = J2.T() * invcov;
00186 const Matrix<S1,S2,Precision> mixed = temp1 * J2;
00187 const int size1 = J1.num_cols();
00188 const int size2 = J2.num_cols();
00189 my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
00190 my_C_inv.slice(index2, index2, size2, size2) += temp2 * J2;
00191 my_C_inv.slice(index1, index2, size1, size2) += mixed;
00192 my_C_inv.slice(index2, index1, size2, size1) += mixed.T();
00193 my_vector.slice(index1, size1) += temp1 * m;
00194 my_vector.slice(index2, size2) += temp2 * m;
00195 }
00196
00197
00198
00199 void compute(){
00200
00201
00202 for(int r=1; r < my_C_inv.num_rows(); r++)
00203 for(int c=0; c < r; c++)
00204 my_C_inv[r][c] = my_C_inv[c][r];
00205
00206 my_decomposition.compute(my_C_inv);
00207 my_mu=my_decomposition.backsub(my_vector);
00208 }
00209
00210
00211
00212 void operator += (const WLS& meas){
00213 my_vector+=meas.my_vector;
00214 my_C_inv += meas.my_C_inv;
00215 }
00216
00217
00218 Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;}
00219
00220 const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;}
00221 Vector<Size,Precision>& get_mu(){return my_mu;}
00222 const Vector<Size,Precision>& get_mu() const {return my_mu;}
00223 Vector<Size,Precision>& get_vector(){return my_vector;}
00224 const Vector<Size,Precision>& get_vector() const {return my_vector;}
00225 Decomposition<Size,Precision>& get_decomposition(){return my_decomposition;}
00226 const Decomposition<Size,Precision>& get_decomposition() const {return my_decomposition;}
00227
00228
00229 private:
00230 Matrix<Size,Size,Precision> my_C_inv;
00231 Vector<Size,Precision> my_vector;
00232 Decomposition<Size,Precision> my_decomposition;
00233 Vector<Size,Precision> my_mu;
00234
00235
00236 WLS( WLS& copyof );
00237 int operator = ( WLS& copyof );
00238 };
00239
00240 }
00241
00242 #endif