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