marqfitp.cpp

二次最优化的C/C++源代码

// MarqFitP.CPP
// verze duben 1998, viz marqfitp.h

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>
#include <iomanip.h>
#ifdef __TURBOC__
  #include <mem.h>
  #include <conio.h>
#endif
#if defined(__GNUC__) && defined(__MSDOS__)
  #include <pc.h>
#endif
#if defined(__STDC__) || defined(__DECCXX)
  #include "ansicpp.h"
  #define stricmp strcmp
#else
  #define COUT_FLUSH
#endif
#include "marqfitp.h"

#ifdef __MSDOS__
  #define PM "\xf1" // plusminus character
  #else
  #define PM "+-"
#endif

const float FitMachEps     = 1.2e-7;   // FLT_EPSILON
const float FitSqrtMachEps = 3.5e-4;
/*
const double FitMachEps     = 2.3e-16;  // FLT_EPSILON
const double FitSqrtMachEps = 1.52e-8;
*/
int IndexLongMat ( int I, int J )
{
// index pro spodni trojuhelnikovou sym. matici
  if (J>I) return (J-1)*J / 2 + I;
  else return (I-1)*I / 2 + J;
}

int KbdIntr()                 // preruseni stiskem ESC
{
// (ReadKey()==0) and (ReadKey==$4F )  //to je END key - plati to i o !PC ?
  if (kbhit())
    if (getch()=='\x1b')
      {
	cout << "FITTING ABORTED BY ESC.\n\a" COUT_FLUSH;
        return 1;
      }
  return 0;
}

#ifdef ElaTime
char* TimeToString (char* S, double time)
{
  int h=int(time/3600); time-=h*3600;
  int m=int(time/60); time-=m*60;
  sprintf(S,"%02d:%02d:%05.2lf",h,m,time);
  return S;
}
#endif

// --- CHODEC (v 2.0/150188/jc) ---
void ChoDec (int dimA, longvecfloat *A, int& Res)
{
  int I,J,K,M,Q;
  longvecfloat S;
  Res=1; J=1;
  do {
    Q=J*(J+1) / 2;
    if (J>1)
    {
      for (I=J; I<=dimA; I++)
      {
        M=I*(I-1) / 2 +J; S=A[M];
        for (K=1; K<=J-1; K++) S -= A[M-K]*A[Q-K];
        A[M]=S;
      }
    }
    if (A[Q]<=0.) Res=0;   // Matice neni poz. definitni
    else
    {
      S=sqrt(A[Q]);
      for (I=J; I<=dimA; I++)
      {
        M=I*(I-1) / 2 +J; A[M] /= S;
      }
    }
  J++;
  } while (!(!Res || J>dimA));
}

// --- CHOSOL (v 2.0/150188/jc) ---
void ChoSol ( int dimA, longvecfloat *A, parfloat *B)
{
  int I,J,Pom,Q;
  B[1] /= A[1];
  if (dimA>1)
  {
    Q=1;
    for (I=2; I<=dimA; I++)
    {
      for (J=1; J<=I-1; J++)
        B[I] -= A[++Q]*B[J];
      B[I] /= A[++Q];
    }
  }
  Pom=dimA*(dimA+1) / 2; B[dimA] /= A[Pom];
  if (dimA>1)
  {
    for (I=dimA; I>=2; I--)
    {
      Q=I*(I-1) / 2;
      for (J=1; J<=I-1; J++)
        B[J] -= B[I]*A[Q+J];
      B[I-1] /= A[Q];
    }
  }
}

void InvSymMatice ( int N, longvecfloat *A, int& Eval1 )
{
// inverze symetricke matice
  int I,J,K,Q,M;
  longvecfloat S,T;
  longvecfloat *X;
  X = new longvecfloat [N*(N+1)/2+2];
  if (!X)
  {
    Eval1=0; return;
  }
  for (K=N; K>=1; K--)
  {
    S=A[1];
    if (S<=0)
    {
      cerr << "\n!! Warning: Hess matrix is badly conditioned !!\n";
      Eval1=0; return;
    }
    M=1;
    for (I=2; I<=N; I++)
    {
      Q=M; M += I; T=A[Q+1];
      X[I]=-T/S; // note use temporary X
      if (I>K) X[I]=-X[I];
      for (J=Q+2; J<=M; J++)
        A[J-I]=A[J]+T*X[J-Q];
    }
    Q--; A[M]=1./S;
    for (I=2; I<=N; I++)
      A[Q+I]=X[I];
  }
  delete [] X;
}


// --- TMarq ---

TMarq::TMarq ( const char *ATraceFile, int AShowErrors )
{
  ofstream Out3;
  Eval1=0; ShowErrors=AShowErrors; strcpy(TraceFile,ATraceFile);
  if (stricmp(TraceFile,""))
  {
    Out3.open(TraceFile,ios::app);
    Out3 <<"\n===============================\n";
    Out3.close();
  }
#ifdef ElaTime
  beginTime=lastTime=clock();
#endif
}

TMarq::~TMarq() // destructor
{
  delete [] SaveA;
}

// empty function
#ifdef __BORLANDC__
  #pragma argsused
#endif
datafloat TMarq::FitFun
  ( datafloat /* X */, parfloat * /* *B */, int& Fres )
{ Fres=0; return 0.; }

void TMarq::DoJac ( int I, datafloat *X, parfloat *B,
  parfloat *D, int& dres )
{
// zabudovana numericka derivace
  int J;
  parfloat Savebj, ri0, rih, hj;
  ri0=D0[I]; // FitFun(X[I],B,dres);
  for (J=1; J<=n_par; J++)
  {
    hj=(fabs(B[J])+FitSqrtMachEps)*FitSqrtMachEps;
    Savebj=B[J]; B[J] += hj;
    rih=FitFun(X[I],B,dres); cfitfun++;
    if (!dres) return;
    D[J]=(rih-ri0)/hj;
    B[J]=Savebj;
  }
}


void TMarq::Residue ( datafloat& Res, int& Suc,
  datafloat *X, datafloat *Y, datafloat *Vaha, parfloat *B )
{
// Vraci hodnotu Res rezidualniho souctu ctvercu r'.r pro dane b;
//  (' zde i v dalsim znaci transpozici). Dale spocte vektor residui R
  int I=0;
  Res=0; Suc=1; ++cresid;
  while (Suc && ( I<n_dat ))
  {
    _Indx_Mr=(++I);
    D0[I]=FitFun(X[I],B,Suc); cfitfun++;
//save pro num. derivaci. Jede i ve FormMat!!!
    R[I]=D0[I]-Y[I];
//   _MarqRez[I]=R[I];
    Res += Vaha[I]*(R[I]*R[I]);
  }
}

void TMarq::FormMat (int& Suc,longvecfloat *A,datafloat *X,
            datafloat *Vaha,parfloat *B, parfloat *V )
{
//   Vytvari matice J'J , J'r
//   Pri uspesnem dokonceni eval==1, eval==0 indikuje
//  neuspech pri vypoctu Dmarq
  int I,J,K,Q;
  parfloat *Der;
  Der = new parfloat [n_par+1];

  if (!Der)
  {
    Suc=0;
    cerr <<"! NOT ENOUGH MEMORY !\n";
    return;
  }
  memset( A, 0, sizeofA );
  memset( V, 0, sizeofB );
  I=0;  // Vypocet J'J a J'r
  do {
    DoJac (++I,X,B,Der,Suc);
    cderiv += n_par;
    if (!Suc) return;
    for (J=1; J<=n_par; J++)
    {
      V[J] += Vaha[I]*Der[J]*R[I];  // Do vypoctu J'r
      Q=J*(J-1)/2;
      for (K=1; K<=J; ++K)  // Do vypoctu J'J
        A[Q+K] += Vaha[I]*Der[J]*Der[K];
    }
  } while (I!=n_dat);  // or (not Suc)
  delete [] Der;
}

void TMarq::Marq
  ( int _n_dat, datafloat *X, datafloat *Y, datafloat *Vaha,
    int _n_par, parfloat *B, datafloat& SumR, int& Eval, int Print )
{
// vola se druha definice Marq bez ohraniceni parametru B
// parfloat *minB,*maxB; // unused pointers==NULL
Marq(_n_dat,X,Y,Vaha,_n_par,B,NULL,NULL,SumR,Eval,Print,0);
}

void TMarq::Marq
  ( int _n_dat, datafloat *X, datafloat *Y, datafloat *Vaha,
    int _n_par, parfloat *B, parfloat *Bmin, parfloat *Bmax,
    datafloat& SumR, int& Eval, int Print, int b)
{
  const fitfloat Fi  = 1.;    // Faktor pro rozsireni matice J'J
  const fitfloat dec = 0.4;   // Faktor pro zmensovani "lambda"
  const fitfloat inc = 10.;   // Faktor pro zvetsovani "lambda"
  const fitfloat Tol = FitMachEps;  // Velicina rovna macheps - viz Inclam
  StopMarq=0; // new 3.8.96

  int Step1,Konec,prtflg;  // boolean
  int Count;
  n_dat=_n_dat; n_par=_n_par;

  SumY2=0; for (Count=n_dat+1; --Count;
		SumY2+=Y[Count]*Y[Count]*Vaha[Count]);

  R = new datafloat [n_dat+1];
  D0 = new datafloat [n_dat+1];
  parfloat *V, *xr, *SaveB;
  V = new parfloat [n_par+1];
  xr = new parfloat [n_par+1];
  SaveB = new parfloat [n_par+1];

  sizeofB = (n_par+1) * sizeof(parfloat);
  sizeofA = n_par*(n_par+1)/2+1;
  SaveA = new longvecfloat [sizeofA];
  longvecfloat *A;
  A = new longvecfloat [sizeofA];
  sizeofA *= sizeof(longvecfloat);
  if ( !R || !D0 || !V || !xr || !SaveB || !A )
    { cerr<<"! NOT ENOUGH MEMORY !\n"; Eval1=Eval=0;
      return; }

  StopMarq=0;
  Lambda=0.0001;  // Pocatecni hodnota Marquardtova parametru
  cresid=0;       // Pocitadlo volani Residue
  cderiv=0;       // Pocitadlo volani Dmarq
  citer =0;       // Pocitadlo iteraci
  cprint=0;       // Pocitadlo vystupu
  cfitfun=0;      // Pocitadlo vypoctu funkce
  Step1= 1; Konec=0;
// ElapsedTime=0; GetTime(Hour,Min,Sec,Sec100);
//  uz bylo provedeno v Init !

  if (Print<=0)  prtflg=0;
  else  prtflg=1;
  Residue(SumR1,Eval,X,Y,Vaha,B);
  if (prtflg)  Vystup(SumR1,B,0);

  if (Eval)
  {
    do {  // Hlavni iteracni cyklus
      if (Step1)
      {
        citer++; Lambda *= dec; SumR=SumR1;
        if ((prtflg)&&(citer>1))
        {  // Vystup pro sledovani prubehu minimalizace
          cprint++;
          if ((cprint==1) || (cprint % Print==0)) Vystup(SumR,B,Eval);
        }
        FormMat(Eval,A,X,Vaha,B,V);  // Formovani J'J do A, J'r do V
        if (Eval)
	{
          memcpy(SaveA,A,sizeofA);  // Uschova poli A, B
	  memcpy(SaveB,B,sizeofB);
	  InvYes=0;
	}
        else
          Konec=1;
      }
      if (!Konec)
      {
      // vlozeno SolveEqs(xr); // Reseni rovnic do xr
      // reseni  (J'J+lambda*DD)q==-J'r  do xr
        int posdef,first;  // boolean; first indikuje 1.pruchod
        int j,q;
        posdef=0; first=1;
        while (!posdef)
        {
          if (!first)  memcpy(A,SaveA,sizeofA);
          else  first=0;
          for (j=1; j<=n_par;j++)
          {  // Doplneni diagonalnich prvku
	    q=j*(j+1)/2;  // Index diag.prvku
	    A[q]=SaveA[q]*(1.+Lambda)+Fi*Lambda;
	    xr[j]=-V[j];
          }
          ChoDec(n_par,A,posdef);  // Choleskeho rozklad J'J v A
          if (!posdef)
	  {
            Lambda*=inc;  // Provede zvetseni "lambda"
            if (Lambda==0.) Lambda=Tol;
          }
        }
        ChoSol(n_par,A,xr);

        // vlozeno Newb(xr,Count); // Korigovane b a test konvergence
        Count=0;
        for (int i=1; i<=n_par; i++)
        {
          B[i]=SaveB[i]+xr[i];
          if (b)
          {
            if (B[i]<Bmin[i])  B[i]=Bmin[i];
            if (B[i]>Bmax[i])  B[i]=Bmax[i];
          }
	  if (B[i]==SaveB[i])  Count++;
        }
        if (Count==n_par)
          Konec=1;
        else
        {
          Residue(SumR1,Eval,X,Y,Vaha,B);
          if (!Eval || SumR1>=SumR)
	  {
            Lambda *= inc;  // Provede zvetseni "lambda"
            if (Lambda==0.)  Lambda=Tol;
	    Step1=0;
          }
          else  Step1=1;
        }
      }
    if (KbdIntr()) StopMarq=-0x1B; // new 3.8.96
    } while (!(Konec /* || KbdIntr() || */ || StopMarq));
// konec stiskem ESC nebo nastavenim StopMarq
  }
  Eval1=Eval;
  if (prtflg)  Vystup(SumR,B,Eval);
  SumR1=SumR; Eval1=Eval;  // pro statisticke funkce
// v SaveA je Hessian
  if (Eval && !InvYes)
  {
    InvSymMatice(n_par,SaveA,Eval1); InvYes=Eval1;
  }
  if (ShowErrors && !InvYes)  Eval1=0;
// ted je v SaveA kovariancni matice (pro statisticke funkce)
#ifdef ElaTime
// celkovy uplynuly cas
  ofstream Out3(TraceFile,ios::app);
  Out3 <<"*** Elapsed time  ";
  Out3 <<TimeToString(elTime,GetElapsedTime(1)) <<endl;
  Out3.close();
#endif
  delete [] A;
  delete [] SaveB;
  delete [] V;
  delete [] xr;
  delete [] R;
  delete [] D0;
}  // konec Marq()

void TMarq::Vystup ( datafloat SumR, parfloat *B, int Eval ) {
int I;
if (!InvYes)
  if (!ShowErrors || !Eval) Eval1=0;
    else {
      Eval1=1; InvSymMatice(n_par,SaveA,Eval1);
      if (Eval1) InvYes=1; }
#ifdef ElaTime
  double tim=0.1*(int)(10*GetElapsedTime());
#endif
ofstream FOut3; ostream *Out3;
for (int j=0; j<2; ++j) {
  if (j) {
      if (!stricmp(TraceFile,""))  return;
      FOut3.open(TraceFile,ios::app); Out3=&FOut3; }
    else Out3=&cout; // Out3.open("con"); only if PC
  *Out3 <<"*iter="<< citer <<"  fitfun=" <<cfitfun
	<<"\tresid=" <<cresid <<"  deriv=" <<cderiv;
  #ifdef ElaTime
    *Out3<<"\ttime="<<tim <<"s/" <<TimeToString(elTime,GetElapsedTime(1));
    *Out3 <<endl;
  #endif
  *Out3 << " lambda=" << setprecision(4) << Lambda;
  *Out3 << "   \tresiduum=" << setprecision(5) << SumR
	<< "\trel. residuum=" << SumR/SumY2 << endl;
  if (!Eval1) {
      for (I=1; I<=n_par; I++) {
	*Out3<<I<<". param: "<<B[I]<<"     \t";
	if ((I % 2==0) || (I==n_par)) *Out3 << endl; }
      }
    else
      for (Out3->setf(ios::left),I=1; I<=n_par; I++)
	*Out3<<I<<". param: "<<setw(8)<<B[I]<<" \t"PM" "<<GetSigma(I)<<endl;
  if (j) FOut3.close();
    else *Out3 << "" COUT_FLUSH;
  }
}

datafloat TMarq::GetKorelKoef ( int Par1, int Par2 ) {
if (Par1==Par2)  return 1.;
if (!Eval1)
    return 1e38; // _maxflt
  else {
    datafloat Pom =
      SaveA[IndexLongMat(Par1,Par1)] * SaveA[IndexLongMat(Par2,Par2)];
    if (!Pom) return 1e38;  // _maxreal
      else return SaveA[IndexLongMat(Par1,Par2)] / sqrt(Pom);
    }
}

datafloat TMarq::GetDisperze( void ) {
if (!Eval1) return 1e38;  // _maxflt;
  else return SumR1 / (n_dat-n_par);
}

datafloat TMarq::GetSigma ( int Par ) {
if (!Eval1) return 1e38;  // _maxflt
  else
    return sqrt( GetDisperze() * SaveA[IndexLongMat(Par,Par)] );
}

void TMarq::PomInfoVystup ( ostream& Out ) {
if (!Eval1)  Out<<"! FIT HAS NOT FINISHED SUCCESSFULLY !\n";
Out <<"Iterace=" <<citer <<"  Funkci=" <<cfitfun <<"  Derivaci=" <<cderiv
    <<"  Residui=" <<cresid <<"  Marquardtovo lambda=" <<Lambda <<endl;
#ifdef ElaTime
  Out <<"Spotrebovany cas=" <<GetElapsedTime(1) <<endl;
#endif
}

void TMarq::InfoVystup ( ostream& Out ) {
PomInfoVystup(Out);
if (*TraceFile) {
  ofstream Out3; Out3.open(TraceFile,ios::app); Out3 << endl;
  PomInfoVystup(Out3); Out3.close(); }
}

#ifdef ElaTime
double TMarq::GetElapsedTime( int i ) {
clock_t x;
endTime=clock();
if (i==1) lastTime=beginTime;
x=endTime-lastTime;
lastTime=endTime;
return x/CLK_TCK;
}
#endif


// --- TMarqData ---

// empty function
void TMarqData::FitFunData
  ( datafloat * /* *X */, parfloat * /* *B */, datafloat * /* *D */, int& Fres )
{ Fres=0; }

void TMarqData::DoJacData ( int Par, datafloat *X,
  parfloat *B, datafloat *Der, int& dres ) {
// Numericka derivace. D0 musi byt jiz spoctene
  int K;
  datafloat Savebj,hj;
  Savebj=B[Par];
  hj=(fabs(B[Par])+FitSqrtMachEps)*FitSqrtMachEps;
  B[Par] += hj;
  FitFunData ( X,B,Der,dres ); cfitfun++;
  for (K=1; K<=n_dat; K++)  Der[K]=(Der[K]-D0[K])/hj;
  B[Par]=Savebj;
}

void TMarqData::Residue ( datafloat& Res, int& Suc,
  datafloat *X, datafloat *Y, datafloat *Vaha, parfloat *B )
{
  int I;
  Res=0; cresid++;
  FitFunData( X,B,D0,Suc ); cfitfun++;
  if (!Suc)  return;
  _Indx_Mr=n_dat;
  for (I=1; I<=n_dat; I++)
  {
    R[I]=D0[I]-Y[I];
    // _MarqRez[I]=R[I];
    Res += Vaha[I]*(R[I]*R[I]);
   }
}

void TMarqData::FormMat ( int& Suc, longvecfloat *A,
  datafloat *X, datafloat *Vaha, parfloat *B, parfloat *V )
{
  // typedef Data BigDer[NumPar];
  int K,I,J,Q;
  parfloat *BigDer = new parfloat [n_par*n_dat];
  if (!BigDer)
  {
    Suc=0;
    cerr <<"! NOT ENOUGH MEMORY !\n";
    return;
  }
  memset ( A, 0, sizeofA );
  memset ( V, 0, sizeofB );
// FitFunData( X,D0,B,Suc ); if not Suc then exit;
// D0 se spocetlo v casti Residue a B se nehlo
  parfloat *PKI, *gPI, *PJI, Pom;;
  for (K=1,PKI=BigDer; K<=n_par; K++,PKI+=n_dat)
  {
    DoJacData( K,X,B,PKI-1,Suc );  // PKI==BigDer[K] ale s nulou !
    if (!Suc)
    {
      delete [] BigDer; return;
    }
  }
  cderiv += n_par;
  for (I=1,gPI=BigDer; I<=n_dat; I++,gPI++)
  {
    PJI=gPI;
    for (J=1; J<=n_par; J++,PJI+=n_dat)
    {
      Pom = Vaha[I]*(*PJI);
      V[J] += Pom*R[I];  // +=Vaha[I]*BigDer[J][I]*R[I];
      Q=J*(J-1)/2;
      PKI=BigDer+I-1;
      for (K=1; K<=J; K++,PKI+=n_dat)
	A[Q+K]+=Pom*(*PKI);  // +=Vaha[I]*BigDer[J][I]*BigDer[K][I];
    }
  }
  delete [] BigDer;
}
// konec MarqFitP.CPP