TooN: irls.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

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 namespace TooN {
    00039
    00040     /// Robust reweighting (type I) for IRLS.
    00041     /// A reweighting class with \f$w(x)=\frac{1}{\sigma + |x|}\f$.
    00042     /// This structure can be passed as the second template argument in IRLS.
    00043     /// @ingroup gEquations
    00044     template<typename Precision>
    00045     struct RobustI {
    00046         void set_sd(Precision x){ sd_inlier = x;} ///<Set the noise standard deviation.
    00047         double sd_inlier; ///< The inlier standard deviation, \f$\sigma\f$.
    00048         inline Precision reweight(Precision x) {return 1/(sd_inlier+fabs(x));}  ///< Returns \f$w(x)\f$.
    00049         inline Precision true_scale(Precision x) {return reweight(x) - fabs(x)*reweight(x)*reweight(x);}  ///< Returns \f$w(x) + xw'(x)\f$.
    00050         inline Precision objective(Precision x) {return fabs(x) + sd_inlier*log(sd_inlier*reweight(x));}  ///< Returns \f$\int xw(x)dx\f$.
    00051     };
    00052
    00053     /// Robust reweighting (type II) for IRLS.
    00054     /// A reweighting class with \f$w(x)=\frac{1}{\sigma + x^2}\f$.
    00055     /// This structure can be passed as the second template argument in IRLS.
    00056     /// @ingroup gEquations
    00057     template<typename Precision>
    00058     struct RobustII {
    00059         void set_sd(Precision x){ sd_inlier = x*x;} ///<Set the noise standard deviation.
    00060         Precision sd_inlier; ///< The inlier standard deviation squared, \f$\sigma\f$.
    00061         inline Precision reweight(Precision d){return 1/(sd_inlier+d*d);} ///< Returns \f$w(x)\f$.
    00062         inline Precision true_scale(Precision d){return d - 2*d*reweight(d);} ///< Returns \f$w(x) + xw'(x)\f$.
    00063         inline Precision objective(Precision d){return 0.5 * log(1 + d*d/sd_inlier);} ///< Returns \f$\int xw(x)dx\f$.
    00064     };
    00065
    00066     /// A reweighting class representing no reweighting in IRLS.
    00067     /// \f$w(x)=1\f$
    00068     /// This structure can be passed as the second template argument in IRLS.
    00069     /// @ingroup gEquations
    00070     template<typename Precision>
    00071     struct ILinear {
    00072         void set_sd(Precision){} ///<Set the noise standard deviation (does nothing).
    00073         inline Precision reweight(Precision d){return 1;} ///< Returns \f$w(x)\f$.
    00074         inline Precision true_scale(Precision d){return 1;} ///< Returns \f$w(x) + xw'(x)\f$.
    00075         inline Precision objective(Precision d){return d*d;} ///< Returns \f$\int xw(x)dx\f$.
    00076     };
    00077
    00078     ///A reweighting class where the objective function tends to a
    00079     ///fixed value, rather than infinity. Note that this is not therefore
    00080     ///a proper distribution since its integral is not finite. It is considerably
    00081     ///more efficient than RobustI and II, since log() is not used.
    00082     /// @ingroup gEquations
    00083     template<typename Precision>
    00084     struct RobustIII {
    00085
    00086         void set_sd(Precision x){ sd_inlier = x*x;} ///<Set the noise standard deviation.
    00087         Precision sd_inlier; ///< Inlier standard deviation squared.
    00088         /// Returns \f$w(x)\f$.
    00089         Precision reweight(Precision x) const
    00090         {
    00091             double d = (1 + x*x/sd_inlier);
    00092             return 1/(d*d);
    00093         }
    00094         ///< Returns \f$\int xw(x)dx\f$.
    00095         Precision objective(Precision x) const
    00096         {
    00097             return x*x / (2*(1 + x*x/sd_inlier));
    00098         }
    00099     };
    00100
    00101     /// Performs iterative reweighted least squares.
    00102     /// @param Size the size
    00103     /// @param Reweight The reweighting functor. This structure must provide reweight(),
    00104     /// true-scale() and objective() methods. Existing examples are  Robust I, Robust II and ILinear.
    00105     /// @ingroup gEquations
    00106     template <int Size, typename Precision, template <typename Precision> class Reweight>
    00107     class IRLS
    00108         : public Reweight<Precision>,
    00109           public WLS<Size,Precision>
    00110     {
    00111     public:
    00112         IRLS(int size=Size):
    00113             WLS<Size,Precision>(size),
    00114             my_true_C_inv(Zeros(size))
    00115         {
    00116             my_residual=0;
    00117         }
    00118
    00119         template<int Size2, typename Precision2, typename Base2>
    00120         inline void add_mJ(Precision m, const Vector<Size2,Precision2,Base2>& J) {
    00121             SizeMismatch<Size,Size2>::test(my_true_C_inv.num_rows(), J.size());
    00122
    00123             Precision scale = Reweight<Precision>::reweight(m);
    00124             Precision ts = Reweight<Precision>::true_scale(m);
    00125             my_residual += Reweight<Precision>::objective(m);
    00126
    00127             WLS<Size>::add_mJ(m,J,scale);
    00128
    00129             Vector<Size,Precision> scaledm(m*ts);
    00130
    00131             my_true_C_inv += scaledm.as_col() * scaledm.as_row();
    00132
    00133         }
    00134
    00135         void operator += (const IRLS& meas){
    00136             WLS<Size>::operator+=(meas);
    00137             my_true_C_inv += meas.my_true_C_inv;
    00138         }
    00139
    00140
    00141         Matrix<Size,Size,Precision>& get_true_C_inv() {return my_true_C_inv;}
    00142         const Matrix<Size,Size,Precision>& get_true_C_inv()const {return my_true_C_inv;}
    00143
    00144         Precision get_residual() {return my_residual;}
    00145
    00146         void clear(){
    00147             WLS<Size,Precision>::clear();
    00148             my_residual=0;
    00149             my_true_C_inv = Zeros;
    00150         }
    00151
    00152     private:
    00153
    00154         Precision my_residual;
    00155
    00156         Matrix<Size,Size,Precision> my_true_C_inv;
    00157
    00158         // comment out to allow bitwise copying
    00159         IRLS( IRLS& copyof );
    00160         int operator = ( IRLS& copyof );
    00161     };
    00162
    00163 }
    00164
    00165 #endif