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;
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;
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
00100
00101
00102
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 ) {
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 }