examples/calibrate3d/TooN/irls.h

00001 /*                       
00002          Copyright (C) 2005 Tom Drummond
00003 
00004      This library is free software; you can redistribute it and/or
00005      modify it under the terms of the GNU Lesser General Public
00006      License as published by the Free Software Foundation; either
00007      version 2.1 of the License, or (at your option) any later version.
00008 
00009      This library is distributed in the hope that it will be useful,
00010      but WITHOUT ANY WARRANTY; without even the implied warranty of
00011      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012      Lesser General Public License for more details.
00013 
00014      You should have received a copy of the GNU Lesser General Public
00015      License along with this library; if not, write to the Free Software
00016      Foundation, Inc.
00017      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
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   // comment out to allow bitwise copying
00108   IRLS( IRLS& copyof );
00109   int operator = ( IRLS& copyof );
00110 };
00111 
00112 #ifndef TOON_NO_NAMESPACE
00113 }
00114 #endif
00115 
00116 #endif

Generated on Fri Feb 22 18:26:54 2008 for QVision by  doxygen 1.5.3