00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef __IRLS_H
00020 #define __IRLS_H
00021
00022 #include <TooN/wls.h>
00023 #include <cassert>
00024 #include <cmath>
00025
00026 #ifndef TOON_NO_NAMESPACE
00027 namespace TooN {
00028 #endif
00029
00034 struct RobustI {
00035 double sd_inlier;
00036 inline double reweight(double x) {return 1/(sd_inlier+fabs(x));}
00037 inline double true_scale(double x) {return reweight(x) - fabs(x)*reweight(x)*reweight(x);}
00038 inline double objective(double x) {return fabs(x) + sd_inlier*log(sd_inlier*reweight(x));}
00039 };
00040
00045 struct RobustII {
00046 double sd_inlier;
00047 inline double reweight(double d){return 1/(sd_inlier+d*d);}
00048 inline double true_scale(double d){return d - 2*d*reweight(d);}
00049 inline double objective(double d){return 0.5 * log(1 + d*d/sd_inlier);}
00050 };
00051
00056 struct ILinear {
00057 inline double reweight(double d){return 1;}
00058 inline double true_scale(double d){return 1;}
00059 inline double objective(double d){return d*d;}
00060 };
00061
00062
00068 template <int Size, class Reweight>
00069 class IRLS
00070 : public Reweight,
00071 public WLS<Size>
00072 {
00073 public:
00074 IRLS(){Identity(my_true_C_inv,0);my_residual=0;}
00075
00076 inline void add_df(double d, const Vector<Size>& f) {
00077 double scale = Reweight::reweight(d);
00078 double ts = Reweight::true_scale(d);
00079 my_residual += Reweight::objective(d);
00080
00081 WLS<Size>::add_df(d,f,scale);
00082
00083 for(int i=0; i<Size; i++){
00084 for(int j=0; j<Size; j++){
00085 my_true_C_inv[i][j]+=f[i]*f[j]*ts;
00086 }
00087 }
00088 }
00089
00090 void operator += (const IRLS& meas){
00091 WLS<Size>::operator+=(meas);
00092 my_true_C_inv += meas.my_true_C_inv;
00093 }
00094
00095
00096 Matrix<Size,Size,RowMajor>& get_true_C_inv() {return my_true_C_inv;}
00097 const Matrix<Size,Size,RowMajor>& get_true_C_inv()const {return my_true_C_inv;}
00098
00099 double get_residual() {return my_residual;}
00100
00101 private:
00102
00103 double my_residual;
00104
00105 Matrix<Size,Size,RowMajor> my_true_C_inv;
00106
00107
00108 IRLS( IRLS& copyof );
00109 int operator = ( IRLS& copyof );
00110 };
00111
00112 #ifndef TOON_NO_NAMESPACE
00113 }
00114 #endif
00115
00116 #endif