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
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
00120 IRLS( IRLS& copyof );
00121 int operator = ( IRLS& copyof );
00122 };
00123
00124 #ifndef TOON_NO_NAMESPACE
00125 }
00126 #endif
00127
00128 #endif