00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00140 WLS( WLS& copyof );
00141 int operator = ( WLS& copyof );
00142 };
00143
00144
00145
00146
00147
00148
00149
00150
00151
00155 template<>
00156 class WLS<-1> {
00157 public:
00159
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
00250 SVD<> my_svd(my_C_inv);
00251
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
00273 WLS( WLS& copyof );
00274 int operator = ( WLS& copyof );
00275
00276
00277
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