GFDaf.cxx

Go to the documentation of this file.

/* Copyright 2008-2010, Technische Universitaet Muenchen,
   Authors: Christian Hoeppner & Sebastian Neubert

   This file is part of GENFIT.

   GENFIT is free software: you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as published
   by the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   GENFIT is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public License
   along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
  The DAF algorithm stems from the following sources:
  R. Fruehwirth & A. Strandlie,
  Computer Physics Communications 120 (199) 197-214
  &
  CERN thesis: Dissertation by Matthias Winkler 
 */


#include "larreco/Genfit/GFDaf.h"
#include "larreco/Genfit/GFException.h"

#include "TMath.h"
#include "math.h"

#include "larreco/Genfit/GFTrack.h"
#include "larreco/Genfit/GFAbsRecoHit.h"
#include "larreco/Genfit/GFAbsTrackRep.h"
#include "larreco/Genfit/GFException.h"
#include "larreco/Genfit/GFAbsRecoHit.h"
#include "larreco/Genfit/GFAbsTrackRep.h"
  
#define COVEXC "cov_is_zero"


genf::GFDaf::GFDaf():fBlowUpFactor(500.){
  setProbCut(0.01);
  setBetas(81,8,4,1,1,1);
}


genf::GFDaf::~GFDaf(){;}

void genf::GFDaf::processTrack(GFTrack* trk){
  trk->clearBookkeeping();


  
  unsigned int nreps=trk->getNumReps();
  unsigned int nhits=trk->getNumHits();
  
  for(unsigned int i=0; i<nreps; ++i){ 
    GFBookkeeping *bk = trk->getBK(i);
    bk->setNhits(nhits);
    bk->bookMatrices("fPredSt");
    bk->bookMatrices("fPredCov");
    bk->bookMatrices("bPredSt");
    bk->bookMatrices("bPredCov");
    bk->bookMatrices("H");
    bk->bookMatrices("m");
    bk->bookMatrices("V");
    bk->bookGFDetPlanes("pl");  
    bk->bookMatrices("smooResid");
    bk->bookNumbers("p",1.);
  }

  //plane grouping
  std::vector< std::vector<int>* > planes;
  trk->getHitsByPlane(planes);
  int nPlanes = planes.size();

  trk->clearFailedHits();
  for(unsigned int iBeta=0; iBeta<fBeta.size(); iBeta++){
    //std::cout << "@@ beta " << fBeta.at(iBeta) << std::endl;
    if(iBeta>0) blowUpCovs(trk);
    for(int ipl=0; ipl<nPlanes; ++ipl){
      //std::cout << "@@ plane " << ipl << std::endl;
      std::vector< GFAbsRecoHit* > hits;
      unsigned int nhitsInPlane = planes.at(ipl)->size();
      for(unsigned int ihitInPl=0;ihitInPl<nhitsInPlane;++ihitInPl){
        hits.push_back( trk->getHit(planes.at(ipl)->at(ihitInPl)) );
      }
      //now hits contains pointers to all hits in the next plane
      //for non-planar detectors this will always be just one hit
      for(unsigned int irep=0; irep<nreps; ++irep){
        GFAbsTrackRep* rep=trk->getTrackRep(irep);
        if(rep->getStatusFlag()!=0) continue;
        //std::cout << "@@ rep " << irep << std::endl;
        GFBookkeeping *bk = trk->getBK(irep);
        if(bk->hitFailed(planes.at(ipl)->at(0))>0) continue;
        GFDetPlane pl;
        // get prototypes for matrices
        int repDim=rep->getDim();
        TMatrixT<Double_t> state(repDim,1);
        TMatrixT<Double_t> cov(repDim,repDim);;

        if(ipl>0 || (ipl==0 && iBeta==0) ){
          // get the (virtual) detector plane
          //std::cout << "forward ### iBeta, ipl, irep " << iBeta<< " " << ipl << " " << irep << std::endl;
          //      rep->getState().Print();
          try{
            hits.at(0);
            pl=hits.at(0)->getDetPlane(rep);
            //do the extrapolation
            rep->extrapolate(pl,state,cov);

            if(cov[0][0]<1.E-50){
              GFException exc(COVEXC,__LINE__,__FILE__);
              throw exc;
            }
          }
          catch (GFException& exc){
            std::cerr << exc.what();
            exc.info();
            for(unsigned int i=0;i<planes.at(ipl)->size();++i) trk->addFailedHit(irep,planes.at(ipl)->at(i));
            if(exc.isFatal()){
              rep->setStatusFlag(1);
              //abort();
            }
            continue;//go to next rep immediately
          }
        }
        else{
          pl = rep->getReferencePlane();
          state = rep->getState();
          cov = rep->getCov();
        }
        //the H matrix has to be identical for all hits in the plane
        TMatrixT<Double_t> H=hits.at(0)->getHMatrix(rep);
        bk->setMatrix("fPredSt",planes.at(ipl)->at(0),state);
        bk->setMatrix("fPredCov",planes.at(ipl)->at(0),cov);

        TMatrixT<Double_t> covInv;
        invertMatrix(cov,covInv);

        TMatrixT<Double_t> Htransp(H);
        Htransp.T();
        
        bk->setMatrix("H",planes.at(ipl)->at(0),H);
        bk->setDetPlane("pl",planes.at(ipl)->at(0),pl);

        TMatrixT<Double_t> stMod(state.GetNrows(),1);   

        double sumPk(0);
        for(unsigned int ihit=0;ihit<nhitsInPlane;++ihit){
          double pki;
          bk->getNumber("p",planes.at(ipl)->at(ihit),pki);        
          sumPk+=pki;
        }
        TMatrixT<Double_t> V = hits.at(0)->getHitCov(pl);
        TMatrixT<Double_t> Vinv;
        invertMatrix(V,Vinv);
        bk->setMatrix("V",planes.at(ipl)->at(0),V);
        for(unsigned int ihit=0;ihit<nhitsInPlane;++ihit){
          //std::cout << "%%%% forward hit " << planes.at(ipl)->at(ihit) << std::endl;

          TMatrixT<Double_t> m = hits.at(ihit)->getHitCoord(pl);
          bk->setMatrix("m",planes.at(ipl)->at(ihit),m);

          double pki;
          bk->getNumber("p",planes.at(ipl)->at(ihit),pki);
          TMatrixT<Double_t> Gain = calcGain(cov,V,H,sumPk);
          //std::cout << "using weight " << pki << std::endl;
          stMod += pki*Gain*(m-H*state);
        }
        state += stMod;
        invertMatrix( covInv + sumPk*Htransp*Vinv*H ,cov);
        rep->setData(state,pl,&cov);

      }//loop over reps
    }//loop over planes for forward fit
    blowUpCovs(trk);
    //now do the loop over the planes in backward direction
    for(int ipl=nPlanes-1; ipl>=0; --ipl){
      //std::cout << "@bw@ plane " << ipl << std::endl;
      std::vector< GFAbsRecoHit* > hits;
      unsigned int nhitsInPlane = planes.at(ipl)->size();
      for(unsigned int ihitInPl=0;ihitInPl<nhitsInPlane;++ihitInPl){
        hits.push_back( trk->getHit(planes.at(ipl)->at(ihitInPl)) );
      }
      //now hits contains pointers to all hits in the next plane
      //for non-planar detectors this will always be just one hit
      for(unsigned int irep=0; irep<nreps; ++irep){
        GFAbsTrackRep* rep=trk->getTrackRep(irep);
        if(rep->getStatusFlag()!=0) continue;
        //std::cout << "@bw@ rep " << irep << std::endl;
        GFBookkeeping *bk = trk->getBK(irep);
        if(bk->hitFailed(planes.at(ipl)->at(0))>0) continue;
        // get prototypes for matrices
        int repDim=rep->getDim();
        TMatrixT<Double_t> state(repDim,1);
        TMatrixT<Double_t> cov(repDim,repDim);;

        TMatrixT<Double_t> H;
        bk->getMatrix("H",planes.at(ipl)->at(0),H);
        TMatrixT<Double_t> Htransp(H);
        Htransp.T();
        GFDetPlane pl;
        bk->getDetPlane("pl",planes.at(ipl)->at(0),pl);
        //std::cout << "backward ### iBeta, ipl, irep " << iBeta<< " " << ipl << " " << irep << std::endl;
        if(ipl<(nPlanes-1)){
          try{
            //do the extrapolation
            rep->extrapolate(pl,state,cov);
            if(cov[0][0]<1.E-50){
              GFException exc(COVEXC,__LINE__,__FILE__);
              throw exc;
            }
          }
          catch(GFException& exc){
            //std::cout << __FILE__ << " " << __LINE__ << std::endl;
            std::cerr << exc.what();
            exc.info();
            //TAG
            for(unsigned int i=0;i<planes.at(ipl)->size();++i) trk->addFailedHit(irep,planes.at(ipl)->at(i));
            if(exc.isFatal()){
              rep->setStatusFlag(1);
            }
            continue;//go to next rep immediately
          }
        }
        else{
          state = rep->getState();
          cov = rep->getCov();
        }
        TMatrixT<Double_t> covInv;
        invertMatrix(cov,covInv);

        bk->setMatrix("bPredSt",planes.at(ipl)->at(0),state);
        bk->setMatrix("bPredCov",planes.at(ipl)->at(0),cov);
        

        TMatrixT<Double_t> stMod(state.GetNrows(),1);

        double sumPk(0);
        for(unsigned int ihit=0;ihit<nhitsInPlane;++ihit){
          double pki;
          bk->getNumber("p",planes.at(ipl)->at(ihit),pki);        
          sumPk+=pki;
        }

        TMatrixT<Double_t> V;
        bk->getMatrix("V",planes.at(ipl)->at(0),V);
        TMatrixT<Double_t> Vinv;
        invertMatrix(V,Vinv);


        for(unsigned int ihit=0;ihit<nhitsInPlane;++ihit){
          //std::cout << "%%bw%% hit " << planes.at(ipl)->at(ihit) << std::endl;
          TMatrixT<Double_t> m;
          bk->getMatrix("m",planes.at(ipl)->at(ihit),m);
          
          double pki;
          bk->getNumber("p",planes.at(ipl)->at(ihit),pki);
          TMatrixT<Double_t> Gain = calcGain(cov,V,H,sumPk);
          //std::cout << "using pki " << pki << std::endl;

          stMod += pki*Gain*(m-H*state);
        }
        state += stMod;
        invertMatrix( covInv + sumPk*Htransp*Vinv*H , cov);

        rep->setData(state,pl,&cov);
      }//done with loop over reps
    }//loop over planes for backward
    //calculate smoothed states and covs
    TMatrixT<Double_t> fSt,fCov,bSt,bCov,smooSt,smooCov,fCovInv,bCovInv,m,H,smooResid;
    for(int ipl=0; ipl<nPlanes; ++ipl){
      for(unsigned int irep=0; irep<nreps; ++irep){
        if(trk->getTrackRep(irep)->getStatusFlag()!=0) continue;
        GFBookkeeping *bk = trk->getBK(irep);
        if(bk->hitFailed(planes.at(ipl)->at(0))>0) continue;
        bk->getMatrix("fPredSt",planes.at(ipl)->at(0),fSt);
        bk->getMatrix("bPredSt",planes.at(ipl)->at(0),bSt);
        bk->getMatrix("fPredCov",planes.at(ipl)->at(0),fCov);
        bk->getMatrix("bPredCov",planes.at(ipl)->at(0),bCov);
        fCovInv.ResizeTo(fCov);
        bCovInv.ResizeTo(bCov);
        invertMatrix(fCov,fCovInv);
        invertMatrix(bCov,bCovInv);
        smooCov.ResizeTo(fCov);
        invertMatrix( fCovInv + bCovInv , smooCov);
        smooSt.ResizeTo(fSt.GetNrows(),1);
        smooSt = smooCov * (fCovInv*fSt + bCovInv*bSt);
        //std::cout << "#@#@#@#@#@# smooResid ipl, irep " << ipl << " " << irep << std::endl;
        bk->getMatrix("H",planes.at(ipl)->at(0),H);
        for(unsigned int ihit=0;ihit<planes.at(ipl)->size();++ihit){
          //TAG
          bk->getMatrix("m",planes.at(ipl)->at(ihit),m);
          smooResid.ResizeTo(m.GetNrows(),1);
          smooResid = m - H*smooSt;
          //std::cout << "%%%% hit " << planes.at(ipl)->at(ihit) << std::endl;
          //smooResid.Print();
          bk->setMatrix("smooResid",planes.at(ipl)->at(ihit),smooResid);
        }
      }
    }//end loop over planes for smoothed state calculation

    //calculate the new weights
    if(iBeta!=fBeta.size()-1){//dont do it for the last beta, ause fitting will stop
      for(unsigned int irep=0; irep<nreps; ++irep){
        GFBookkeeping *bk = trk->getBK(irep);
        for(int ipl=0; ipl<nPlanes; ++ipl){
          if(trk->getTrackRep(irep)->getStatusFlag()!=0) continue;
          if(bk->hitFailed(planes.at(ipl)->at(0))>0) continue;
          double sumPhi=0.;
          double sumPhiCut=0.;
          std::vector<double> phi;
          TMatrixT<Double_t> V;
          bk->getMatrix("V",planes.at(ipl)->at(0),V);
          V *= fBeta.at(iBeta);
          TMatrixT<Double_t> Vinv; 
          invertMatrix(V,Vinv);
          for(unsigned int ihit=0;ihit<planes.at(ipl)->size();++ihit){
            //TMatrixT<Double_t> smooResid;
            bk->getMatrix("smooResid",planes.at(ipl)->at(ihit),smooResid);
            TMatrixT<Double_t> smooResidTransp(smooResid);
            smooResidTransp.T();
            int dimV = V.GetNrows();
            double detV = V.Determinant();
            TMatrixT<Double_t> expArg = smooResidTransp*Vinv*smooResid;
            if (expArg.GetNrows() != 1)
              throw std::runtime_error("genf::GFDaf::processTrack(): wrong matrix dimensions!");
            double thisPhi = 1./(pow(2.*TMath::Pi(),dimV/2.)*sqrt(detV))*exp(-0.5*expArg[0][0]);
            phi.push_back(thisPhi);
            sumPhi += thisPhi;

            double cutVal = chi2Cuts[dimV];
            if (cutVal <= 1.E-6)
              throw std::runtime_error("genf::GFDaf::processTrack(): chi2 cut too low");
            sumPhiCut += 1./(2*TMath::Pi()*sqrt(detV))*exp(-0.5*cutVal/fBeta.at(iBeta));
          }
          for(unsigned int ihit=0;ihit<planes.at(ipl)->size();++ihit){
            bk->setNumber("p",planes.at(ipl)->at(ihit), phi.at(ihit)/(sumPhi+sumPhiCut));
          }
        }
      }
    }
      
  }//loop over betas    

  
  return;
  
  }

void genf::GFDaf::blowUpCovs(GFTrack* trk){
  int nreps=trk->getNumReps();
  for(int irep=0; irep<nreps; ++irep){
    GFAbsTrackRep* arep=trk->getTrackRep(irep);
    //dont do it for already compromsied reps, since they wont be fitted anyway
    if(arep->getStatusFlag()==0) { 
      TMatrixT<Double_t> cov = arep->getCov();
      for(int i=0;i<cov.GetNrows();++i){
        for(int j=0;j<cov.GetNcols();++j){
          if(i!=j){//off diagonal
            cov[i][j]=0.;
          }
          else{//diagonal
            cov[i][j] = cov[i][j] * fBlowUpFactor;
          }
        }
      }
      arep->setCov(cov);
    }
  }  
}



  
void genf::GFDaf::invertMatrix(const TMatrixT<Double_t>& mat, TMatrixT<Double_t>& inv){
  inv.ResizeTo(mat);
  inv = (mat);
  double det=0;
  inv.Invert(&det);
  if(TMath::IsNaN(det)) {
    GFException e("Daf Gain: det of matrix is nan",__LINE__,__FILE__);
    e.setFatal();
    throw e;
  }
  if(det==0){
    GFException e("cannot invert matrix in Daf - det=0",
                  __LINE__,__FILE__);
    e.setFatal();
    throw e;
  }

}

 TMatrixT<Double_t> genf::GFDaf::calcGain(const TMatrixT<Double_t>& C, 
                                  const TMatrixT<Double_t>& V,
                                  const TMatrixT<Double_t>& H,
                                  const double& p){

   //get C^-1
   TMatrixT<Double_t> Cinv;
   invertMatrix(C,Cinv);
   TMatrixT<Double_t> Vinv;
   invertMatrix(V,Vinv);
   //get H^T
   TMatrixT<Double_t> Htransp(H);
   Htransp.T();
   
   TMatrixT<Double_t> covsum = Cinv + (p*Htransp*Vinv*H);
   TMatrixT<Double_t> covsumInv;
   invertMatrix(covsum,covsumInv);

   return (covsumInv*Htransp*Vinv);
 }


void genf::GFDaf::setProbCut(double val){

  if(fabs(val-0.01)<1.E-10){
    chi2Cuts[1] = 6.63;
    chi2Cuts[2] = 9.21;
    chi2Cuts[3] = 11.34;
    chi2Cuts[4] = 13.23;
    chi2Cuts[5] = 15.09;
  }
  else   if(fabs(val-0.005)<1.E-10){
    chi2Cuts[1] = 7.88;
    chi2Cuts[2] = 10.60;
    chi2Cuts[3] = 12.84;
    chi2Cuts[4] = 14.86;
    chi2Cuts[5] = 16.75;
  }
  else   if(fabs(val-0.001)<1.E-10){
    chi2Cuts[1] = 10.83;
    chi2Cuts[2] = 13.82;
    chi2Cuts[3] = 16.27;
    chi2Cuts[4] = 18.47;
    chi2Cuts[5] = 20.51;
  }
  else{
    throw GFException("genf::GFTrackCand::GFDafsetProbCut(): value not supported", __LINE__, __FILE__)
      .setFatal().setNumbers("value", { val });
  }

}

void genf::GFDaf::setBetas(
  double b1, double b2,
  double b3 /* = -1. */,
  double b4 /* = -1. */,
  double b5 /* = -1. */,
  double b6 /* = -1. */,
  double b7 /* = -1. */,
  double b8 /* = -1. */,
  double b9 /* = -1. */,
  double b10 /* = -1. */
) {
  
  /* // asserting version
  assert(b1>0);fBeta.push_back(b1);
  assert(b2>0 && b2<=b1);fBeta.push_back(b2);
  if(b3>=0.) {
    assert(b3<=b2);fBeta.push_back(b3);
    if(b4>=0.) {
      assert(b4<=b3);fBeta.push_back(b4);
      if(b5>=0.) {
        assert(b5<=b4);fBeta.push_back(b5);
        if(b6>=0.) {
          assert(b6<=b5);fBeta.push_back(b6);
          if(b7>=0.) {
            assert(b7<=b6);fBeta.push_back(b7);
            if(b8>=0.) {
              assert(b8<=b7);fBeta.push_back(b8);
              if(b9>=0.) {
                assert(b9<=b8);fBeta.push_back(b9);
                if(b10>=0.) {
                  assert(b10<=b9);fBeta.push_back(b10);
                }
              }
            }
          }
        }
      }
    }
  }
  */
  if (b1 <= 0)
    throw std::runtime_error("genf::GFDaf::setBetas(): b1 <= 0");
  fBeta.push_back(b1);
  
  if (b2 <= 0 || b2 > b1)
    throw std::runtime_error("genf::GFDaf::setBetas(): b2 in wrong range");
  fBeta.push_back(b2);
  
  if(b3 < 0.) return;
  if (b3 > b2)
    throw std::runtime_error("genf::GFDaf::setBetas(): b3 > b2");
  fBeta.push_back(b3);
  
  if(b4 < 0.) return;
  if (b4 > b3)
    throw std::runtime_error("genf::GFDaf::setBetas(): b4 > b3");
  fBeta.push_back(b4);
  
  if(b5 < 0.) return;
  if (b5 > b4)
    throw std::runtime_error("genf::GFDaf::setBetas(): b5 > b4");
  fBeta.push_back(b5);
  
  if(b6 < 0.) return;
  if (b6 > b5)
    throw std::runtime_error("genf::GFDaf::setBetas(): b6 > b5");
  fBeta.push_back(b6);
  
  if(b7 < 0.) return;
  if (b7 > b6)
    throw std::runtime_error("genf::GFDaf::setBetas(): b7 > b6");
  fBeta.push_back(b7);
  
  if(b8 < 0.) return;
  if (b8 > b7)
    throw std::runtime_error("genf::GFDaf::setBetas(): b8 > b7");
  fBeta.push_back(b8);
  
  if(b9 < 0.) return;
  if (b9 > b8)
    throw std::runtime_error("genf::GFDaf::setBetas(): b9 > b8");
  fBeta.push_back(b9);
  
  if(b10 < 0.) return;
  if (b10 > b9)
    throw std::runtime_error("genf::GFDaf::setBetas(): b10 > b9");
  fBeta.push_back(b10);
  
}