irls.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 
00031 #ifndef __IRLS_H
00032 #define __IRLS_H
00033 
00034 #include <TooN/wls.h>
00035 #include <cassert>
00036 #include <cmath>
00037 
00038 #ifndef TOON_NO_NAMESPACE
00039 namespace TooN {
00040 #endif
00041 
00046 struct RobustI {
00047   double sd_inlier; 
00048   inline double reweight(double x) {return 1/(sd_inlier+fabs(x));}  
00049   inline double true_scale(double x) {return reweight(x) - fabs(x)*reweight(x)*reweight(x);}  
00050   inline double objective(double x) {return fabs(x) + sd_inlier*log(sd_inlier*reweight(x));}  
00051 };
00052 
00057 struct RobustII {
00058   double sd_inlier; 
00059   inline double reweight(double d){return 1/(sd_inlier+d*d);} 
00060   inline double true_scale(double d){return d - 2*d*reweight(d);} 
00061   inline double objective(double d){return 0.5 * log(1 + d*d/sd_inlier);} 
00062 };
00063 
00068 struct ILinear {
00069   inline double reweight(double d){return 1;} 
00070   inline double true_scale(double d){return 1;} 
00071   inline double objective(double d){return d*d;} 
00072 };
00073 
00074 
00080 template <int Size, class Reweight>
00081 class IRLS
00082   : public Reweight,
00083     public WLS<Size>
00084 {
00085 public:
00086   IRLS(){Identity(my_true_C_inv,0);my_residual=0;}
00087 
00088   inline void add_df(double d, const Vector<Size>& f) {
00089     double scale = Reweight::reweight(d);
00090     double ts = Reweight::true_scale(d);
00091     my_residual += Reweight::objective(d);
00092 
00093     WLS<Size>::add_df(d,f,scale);
00094 
00095     for(int i=0; i<Size; i++){
00096       for(int j=0; j<Size; j++){
00097     my_true_C_inv[i][j]+=f[i]*f[j]*ts;
00098       }
00099     }
00100   }
00101 
00102   void operator += (const IRLS& meas){
00103     WLS<Size>::operator+=(meas);
00104     my_true_C_inv += meas.my_true_C_inv;
00105   }
00106 
00107 
00108   Matrix<Size,Size,RowMajor>& get_true_C_inv() {return my_true_C_inv;}
00109   const Matrix<Size,Size,RowMajor>& get_true_C_inv()const {return my_true_C_inv;}
00110 
00111   double get_residual() {return my_residual;}
00112 
00113 private:
00114 
00115   double my_residual;
00116 
00117   Matrix<Size,Size,RowMajor> my_true_C_inv;
00118 
00119   // comment out to allow bitwise copying
00120   IRLS( IRLS& copyof );
00121   int operator = ( IRLS& copyof );
00122 };
00123 
00124 #ifndef TOON_NO_NAMESPACE
00125 }
00126 #endif
00127 
00128 #endif

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