00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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();
00132 Vector<N> temp(invcov*m);
00133 my_vector += J * temp;
00134 my_err += m*temp;
00135 }
00136
00137
00138 void compute(){
00139
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;
00211 double my_extra;
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();
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
00370 my_C_inv += J * invcov * J.T();
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
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;
00454 double my_extra;
00455 };
00456
00457 }
00458
00459 #endif