LMFitter.cxx

Go to the documentation of this file.
00001 
00012 #ifdef _MSC_VER
00013 #include "msdevstudio/MSconfig.h"
00014 #endif
00015 
00016 #include "LMFitter.h"
00017 
00018 #include "NumLinAlg.h"
00019 #include "StatedFCN.h"
00020 
00021 #include <algorithm>
00022 
00023 #include <climits>
00024 #include <cmath>
00025 #include <cassert>
00026 
00027 #ifdef ITERATOR_MEMBER_DEFECT
00028 using namespace std;
00029 #else
00030 using std::abs;
00031 using std::distance;
00032 using std::swap;
00033 using std::vector;
00034 #endif
00035 
00036 using namespace hippodraw::Numeric;
00037 
00038 using namespace hippodraw;
00039 
00040 LMFitter::
00041 LMFitter ( const char * name )
00042   : Fitter ( name ),
00043     m_chi_cutoff ( 0.000001 ),
00044     m_start_lambda ( 0.001 ),
00045     m_lambda_shrink_factor( 9.8 ),
00046     m_lambda_expand_factor( 10.2 )
00047 {
00048 }
00049 
00050 
00051 LMFitter::
00052 LMFitter ( const LMFitter & fitter )
00053   : Fitter ( fitter ),
00054     m_chi_cutoff ( fitter.m_chi_cutoff ),
00055     m_start_lambda ( fitter.m_start_lambda ),
00056     m_lambda_shrink_factor ( fitter.m_lambda_shrink_factor ),
00057     m_lambda_expand_factor ( fitter.m_lambda_expand_factor )
00058 {
00059 }
00060 
00061 
00062 Fitter *
00063 LMFitter::
00064 clone ( ) const
00065 {
00066   return new LMFitter ( *this );
00067 }
00068 
00071 void LMFitter::calcAlpha () 
00072 {
00073   m_fcn -> calcAlphaBeta ( m_alpha, m_beta );
00074   unsigned int num_parms = m_beta.size();
00075 
00076   unsigned int j = 0; // for MS VC++
00077   for ( ; j < num_parms; j++ ) {
00078     for ( unsigned int k = 0; k < j; k++ ) {
00079       m_alpha[k][j] = m_alpha[j][k];
00080     }
00081   }
00082 
00083   j = 0; // for MS VC++
00084   for ( ; j < num_parms; j++ ) {
00085     m_alpha[j][j] *= ( 1.0 + m_lambda );
00086   }
00087 }
00088 
00094 int LMFitter::calcCovariance ( std::vector < std::vector < double > >& cov )
00095 {
00096   m_lambda = 0;
00097   calcAlpha ();
00098     
00099   // Invert the semi hessian to get the error covarience matrix
00100   // Since we use cholesky factorisation we can conclude if the
00101   // given matrix is positive definite or not. So the return value
00102   // is EXIT_SUCCESS if m_alpha is Positive Definite, EXIT_FAILURE otherwise. 
00103   return invertMatrix ( m_alpha, cov ); 
00104 }
00105   
00106 bool LMFitter::solveSystem ()
00107 {
00108   unsigned int num_parms = m_beta.size ();
00109 
00110   vector< int > ipiv ( num_parms, 0 );
00111 
00112   vector< int > indxr ( num_parms, -1 );
00113   vector< int > indxc ( num_parms, -1 );
00114 
00115   unsigned int irow = UINT_MAX;
00116   unsigned int icol = UINT_MAX;
00117 
00118   for ( unsigned int i = 0; i < num_parms; i++ ) {
00119     double big = 0.0;
00120 
00121     for ( unsigned int j = 0; j < num_parms; j++ ) {
00122       if ( ipiv[j] != 1 ) {
00123         
00124         for ( unsigned int k = 0; k < num_parms; k++ ) {
00125           if ( ipiv[k] == 0 ) {
00126             if ( abs ( m_alpha[j][k] ) >= big ) {
00127               big = abs ( m_alpha[j][k] );
00128               irow = j;
00129               icol = k;
00130             }
00131           }
00132           else if ( ipiv[k] > 1 ) {
00133             return false;
00134           }
00135         }
00136       }
00137     }
00138 
00139     if ( irow == UINT_MAX ) { // something is wrong, can't do fit.
00140       return false;
00141     }
00142 
00143     ++ipiv[icol];
00144     if ( irow != icol ) {
00145       for ( unsigned int l = 0; l < num_parms; l++ ) {
00146         swap ( m_alpha[irow][l], m_alpha[icol][l] );
00147       }
00148       swap ( m_beta[irow], m_beta[icol] );
00149     }
00150     indxr[i] = irow;
00151     indxc[i] = icol;
00152     if ( m_alpha[icol][icol] == 0.0 ) {
00153       return false;
00154     }
00155     double pivinv = 1.0 / m_alpha[icol][icol];
00156     m_alpha[icol][icol] = 1.0;
00157 
00158     for ( unsigned int l = 0; l < num_parms; l++ ) {
00159       m_alpha[icol][l] *= pivinv;
00160     }
00161     m_beta[icol] *= pivinv;
00162 
00163     for ( unsigned int ll = 0; ll < num_parms; ll++ ) {
00164       if ( ll != icol ) {
00165         double dum = m_alpha[ll][icol];
00166         m_alpha[ll][icol] = 0.0;
00167 
00168         for ( unsigned int l = 0; l < num_parms; l++ ) {
00169           m_alpha[ll][l] -= m_alpha[icol][l] * dum;
00170         }
00171         m_beta[ll] -= m_beta[icol] * dum;
00172       }
00173     }
00174   }
00175 
00176   for ( int l = num_parms - 1; l >= 0; l-- ) {
00177     if ( indxr[l] != indxc[l] ) {
00178 
00179       for ( unsigned int k = 0; k < num_parms; k++ ) {
00180         swap ( m_alpha[k][indxr[l]], m_alpha[k][indxc[l]] );
00181       }
00182     }
00183   }
00184   return true;
00185 }
00186 
00187 bool LMFitter::calcStep ()
00188 {
00189   calcAlpha ();
00190   bool ok = solveSystem ();
00191 
00192   return ok;
00193 }
00194 
00195 bool LMFitter::calcBestFit ()
00196 {
00197   m_lambda = m_start_lambda;
00198 
00199   int i = 0;
00200   for ( ; i < m_max_iterations; i++ ) {
00201 
00202     double old_chisq = objectiveValue ();
00203 
00204     vector< double > old_parms;
00205     m_fcn -> fillFreeParameters ( old_parms );
00206 
00207     bool ok = calcStep ();
00208     assert ( old_parms.size() == m_beta.size() );
00209 
00210     vector< double > new_parms ( old_parms );
00211     vector< double >::iterator pit = new_parms.begin ( );
00212     vector< double >::iterator dit = m_beta.begin ( );
00213 
00214     while ( pit != new_parms.end () ) {
00215       *pit++ += *dit++;
00216     }
00217     m_fcn -> setFreeParameters ( new_parms );
00218 
00219     double new_chisq = objectiveValue ();
00220 
00221     if ( abs ( old_chisq - new_chisq ) < m_chi_cutoff ) break;
00222 
00223     if ( new_chisq < old_chisq ) {
00224       m_lambda /= m_lambda_shrink_factor; 
00225     }
00226     else {
00227       m_lambda *= m_lambda_expand_factor;
00228       m_fcn -> setFreeParameters ( old_parms );
00229     }
00230 
00231     if ( ! ok ) return ok;
00232   }
00233 
00234   return i < m_max_iterations;
00235 }
00236 
00237 
00238 void
00239 LMFitter::
00240 setFCN ( StatedFCN * fcn )
00241 {
00242   Fitter::setFCN ( fcn );
00243   fcn -> setNeedsDerivatives ( true );
00244 }

Generated for HippoDraw Class Library by doxygen