HarpoKalman.cxx

Go to the documentation of this file.

// 
// File HarpoKalman.cxx
//
#include "HarpoKalman.h"
#include "HarpoConfig.h"
#include "HarpoDetSet.h"
#include "HarpoDebug.h"
#include "HarpoDccEvent.h"
#include "Pmm2Event.h"
#include "HarpoEvent.h"
#include "HarpoRecoEvent.h"

#include "TFile.h"
#include "TStyle.h"
#include "TCanvas.h"
#include "TLatex.h"
#include "TGraph.h"
#include "TF1.h"
#include "TArrow.h"
#include "TLinearFitter.h"
#include "TMath.h"
#include "TSystem.h"

#include <cstdlib>
#include <cstring>
#include <cassert>
#include <fstream>
#include <iostream>


ClassImp(HarpoKalman);

void   HarpoKalman::print()
 {
   unsigned int nd; // number of detectors
   HarpoEventHeader *hdr = fEvt->GetHeader();

   assert(hdr != NULL);
   hdr->print();
   
   for (nd = 0; nd < gkNDetectors; nd++) {
     //     if (fEvt->isdataExist(nd)) {
       HarpoDccMap *plane = fEvt->GetDccMap(nd);
       if (plane != NULL )plane->print();
   }
 }

Int_t  HarpoKalman::InitPlane(TClonesArray* clArray, Int_t plane)
{

  fNtr[plane] = 0;


  for(Int_t itr = 0; itr<NTRACK; itr++){
    fStartIndex[itr] = 0;
    fEndIndex[itr] = 0;
    for(Int_t j = 0; j<10;j++){
      fStartPointX[itr][j] = -1000;
      fStartPointZ[itr][j] = -1000;
      fStartDirX[itr][j] = -1000;
      fStartDirZ[itr][j] = -1000;
    }
  }
  for(Int_t i = 0; i<NADC; i++){
    NTcl[i] = 0;
    for(Int_t  k = 0; k<20; k++)
      Tcl[i][k] = -1;
  }
  for(Int_t i = 0; i<NCHAN; i++){
    NCcl[i] = 0;
    for(Int_t  k = 0; k<20; k++)
      Ccl[i][k] = -1;
  }


  HarpoDccMap* map = fEvt->GetDccMap(plane);

  fQtot = 0;
  fQused = 0;
  for(Int_t i = 0; i<NCHAN; i++){
    for(Int_t j = 0; j<NADC; j++){
      //      fMapTmp[i][j] = map->GetData(i,j);     
      fMapTmp[i][j] = 0;
      if(map->GetData(i,j)>-500) fQtot += map->GetData(i,j);
    }
  }

  Int_t nCl = clArray->GetEntries();
  if(nCl>=4000){
    Warning("InitPlane","Too many clusters %d",nCl);
    return 0;
  }
  // Organise Clusters in layers
  for(Int_t icl = 0; icl<nCl; icl++){
    reused[icl] = 0;
    used[icl] = -1000;
    fTrack[icl] = -1000;
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    Int_t index = cluster->GetIndex();
    //    Info("","index = %d",index);
    //std::cout << plane << "  " << cluster->GetPlane() << std::endl;
    if(cluster->GetPlane() != plane) continue;
    if(cluster->GetQ() < fQmin) continue;
    if(cluster->GetType() == TCLUSTER && cluster->GetWidth() < fWidthMinT) continue;
    if(cluster->GetType() == CCLUSTER && cluster->GetWidth() < fWidthMinC) continue;
    if(cluster->GetWidth() > fWidthMax) continue;
    if(cluster->GetType() == CCLUSTER){
      if(NCcl[index]>=20) continue;
      Ccl[index][NCcl[index]] = icl;
      NCcl[index]++;
    }
    if(cluster->GetType() == TCLUSTER){
      if(NTcl[index]>=20) continue;
      Tcl[index][NTcl[index]] = icl;
      NTcl[index]++;
    }
  }
  
  if(gHarpoDebug>2){
    for(Int_t i = 0; i<NCHAN; i++){
      if(NCcl[i]>0) Info("process","NCcl[%d] = %d",i,NCcl[i]);
    }
    for(Int_t i = 0; i<NADC; i++){
      if(NTcl[i]>0) Info("process","NTcl[%d] = %d",i,NTcl[i]);
    }
  }

  return 1;
}


TArrayI* HarpoKalman::FindNext(TMatrixD X, TMatrixD C, Double_t angle, Int_t iTr, TClonesArray* clArray, TArrayI* arr, Int_t ncl, Int_t plane, Int_t color, Int_t fill, Int_t smooth, Int_t qold)
{
  //  return FindNextClosest(X,C,iTr,clArray,arr,plane,color,fill,smooth);
  return FindNextClosest(X,C,angle,iTr,clArray,arr,ncl,plane,color,fill,smooth,qold);
}


TArrayI* HarpoKalman::FindNextClosest(TMatrixD X, TMatrixD C, Double_t angle, Int_t iTr, TClonesArray* clArray, TArrayI* arr, Int_t ncl, Int_t plane, Int_t color, Int_t fill, Int_t smooth, Int_t qold)
{
  if(gHarpoDebug>2)  Info("FindNextClosest","Test2");
  if(fCanvas[plane]) gSystem->Sleep(0);
  if(gHarpoDebug>2)  Info("FindNextClosest","angle = %g / %d = %g",angle,ncl,angle/ncl);

  TMatrixD F(4,4);      
  for(int i = 0;i<4;i++){
    for(int j =0;j<4;j++){
      F[i][j]=0;
      if(i==j) F[i][j]=1;                               
    }
  }
  TMatrixD Xproj(4,1);
  //  Double_t fNclMin = 20;
  Double_t deltaRes = fResFinder;
  Double_t deltaScat = fScatFinder;
  Double_t theta = fScatKalman; // Multiple scattering
  Int_t fNskipRows = 6;//10;
  if(smooth == 2) fNskipRows = 30;
  Double_t fRangeMax = 5;

  if(gHarpoDebug>2)
    Info("FindNextClosest","%f %f %f %f",X[0][0],X[1][0],X[2][0],X[3][0]);

  Double_t angleOld = TMath::ATan2(X[2][0], X[3][0]); 
  Int_t ntr = 0;
  Double_t distMin = 1000, meanMin = -10000, rangeMin = 0, angleMin = -1000, resMin = -1;
  Int_t iclMin = -1, typeMin = -1, iMin  = -1, kMin = -1, qMin = -1;
  //Info("FindNextClosest","###############################");
  for(Int_t k = 1; k<=fNskipRows; k++){
    if(k>distMin + kMin) break;
    for(Int_t type = 0; type<2; type++){ // look for T- and C-clusters
      if(smooth != 2 && TMath::Abs(X[2+type][0]/X[3-type][0])>fMaxSlopeType) continue; // Angular cut for each cluster type

      // Define next layer
      Int_t I;
      if(X[3-type][0]>0) I = Int_t(X[1-type][0]+0.5) + k;
      else I = Int_t(X[1-type][0]+0.5) - k;

      if(I<0 || (type==CCLUSTER && I>=NCHAN) || (type==TCLUSTER && I>=NADC))  continue; // Out of the map
      Int_t n = 0;
      if(type == TCLUSTER) n = NTcl[I];
      else                 n = NCcl[I];

      F[0][2]=(I-X[1-type][0])*1./X[3-type][0];  // F transforms to the next layer
      F[1][3]=(I-X[1-type][0])*1./X[3-type][0];
      Xproj=F*X;
            
      for(Int_t i = 0; i<n; i++){ // For all clusters in the layer
        if(i == 20) break;
        Int_t icl;
        if(type == TCLUSTER) icl = Tcl[I][i];
        else icl = Ccl[I][i];
        if(smooth == 0 && used[icl] > -1000) continue; // first pass: If already in a track, skip
        if(smooth == 1 && used[icl] > -1000 && reused[icl] != 0) continue; // second pass: If already in a track (except this one), skip
        //      if(smooth == 2 && used[icl] != iTr) continue; // reordering: If not in this track, skip
        
        HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
        if(smooth == 2 && (cluster->GetIdClusterTrack() != iTr && used[icl] != iTr)) continue; // reordering: If not in this track, skip
        if(cluster->GetPlane() != plane) continue;
        if((cluster->GetQuality() & 3) == 3) continue;
        if(cluster->GetType() != type) Warning("FindNextClosest","??? %d != %d",cluster->GetType(), type);
        Double_t mean = cluster->GetMean();
        //Int_t index = cluster->GetIndex();
        Int_t q = cluster->GetQ();
        
        if(gHarpoDebug>2)
          Info("FindNextClosest","#proj: %f %f %f %f",Xproj[0][0],Xproj[1][0],Xproj[2][0],Xproj[3][0]);
        

        Double_t res = GetResolution(cluster);
        Double_t range = deltaRes*res + deltaScat*theta*k;
        if(range>fRangeMax) range = fRangeMax;
        
        Double_t dist = TMath::Abs(mean - Xproj[type][0]);// + k;
        //fHistQQtestVsDist->Fill(q,qold);
        fHistQQtestVsDist->Fill(q*1./qold,dist);
        Double_t angleTmp;
        if(type == 0) angleTmp = TMath::ATan2(mean - X[0][0], I    - X[1][0]   ); 
        else          angleTmp = TMath::ATan2(I    - X[0][0], mean - X[1][0]);
        Double_t dA = fmod(angleTmp - angle/ncl + 4*TMath::Pi(),2*TMath::Pi()) - TMath::Pi();
        if(TMath::Abs(dA)<TMath::Pi()/2 && !(smooth == 1 && used[icl] == iTr)) continue; 

        Double_t dA2 = fmod(angleTmp - angleOld + 4*TMath::Pi(),2*TMath::Pi()) - TMath::Pi();
        //Info("","dA = %g - %g = %g",angleTmp,angle/ncl,dA);
        if(TMath::Abs(dA2)<TMath::Pi()*0.7 && !(smooth == 1 && used[icl] == iTr)) continue; 
        if(smooth == 1 && used[icl] == iTr) dist = 0; // Catch
        //Info("FindNextClosest","** %d %g %g %g %d %d",icl, dist,mean,range,type,I);
        if((dist < range || smooth == 2) && (dist+k<distMin || (dist == 0 && k<=kMin))){
          //if((dist < range) && (dist+k<distMin || (dist == 0 && k<=kMin))){
          distMin = dist;
          iclMin = icl;
          meanMin = mean;
          rangeMin = range;
          typeMin = type;
          iMin = I;
          kMin = k;
          if(smooth == 1) kMin = 100;
          angleMin = angleTmp;
          resMin = res;
          qMin = q;
        }
      }
    }
  }

  fHistQQmin->Fill(qMin*1./qold,distMin);

  // Info("FindNextClosest","%d: %g %d %g %g %d %d %d %g %g",smooth,distMin,iclMin,meanMin,rangeMin,typeMin,iMin,kMin,angleMin,resMin);
  // Info("FindNextClosest","used[%d] = %d",iclMin,used[iclMin]);
   
  if(iclMin!=-1) ntr++;
  else{ // END OF TRACK
    if(smooth == 0 && fStartIndex[iTr]>10){ // end of first pass
      // X[2][0] = -X[2][0];
      // X[3][0] = -X[3][0];

      Int_t index = (fStartIndex[iTr] + 4)%10;
      X[0][0] = fStartPointZ[iTr][index];
      X[1][0] = fStartPointX[iTr][index];
      X[2][0] = -fStartDirZ[iTr][index];
      X[3][0] = -fStartDirX[iTr][index];

      if(gHarpoDebug>1)
        Info("FindNextClosest","End of first pass (Ncl = %d). New seed: (%g, %g, %g, %g)",ncl,X[0][0],X[1][0],X[2][0],X[3][0]);

      TMatrixD Corig(4,4);
      for(int a = 0;a<4;a++){
        for(int b =0;b<4;b++){
          if(a==b){
            if(a<2)
              Corig[a][b]=fResKalman*fResKalman;                                      //initialisation de C
            else
              Corig[a][b]=(2*fResKalman)*(2*fResKalman)+fScatKalman*fScatKalman;
          }
          else
            Corig[a][b]=0;
        }
      }
      //      ncl++;
      //      Double_t angleorig = TMath::ATan2(X[2][0],X[3][0]);
      //      arr = FindNextClosest(X, Corig, angleorig, iTr, clArray, arr, 1, plane, color, fill, 1);
      arr = FindNextClosest(X, Corig, -angle, iTr, clArray, arr, ncl, plane, color, fill, 1, qMin);
    }else{ // end of reverse pass
      //      Int_t Ncl = 0;
      if(gHarpoDebug>1)
        Info("FindNextClosest","End of last pass (%d clusters)",ncl);
      // while(arr->At(Ncl) != 0){
      //        Ncl++;
      // }
      // std::cout << "*** " << iTr << " => " << Ncl << " ***" << std::endl;
    }
    return arr;
  }
  
  if(gHarpoDebug>2)
    Info("FindNextClosest","Add New Cluster");


  TMatrixD H(2,4);
  TMatrixD Ht(2,4);
  TMatrixD Q(4,4);
  TMatrixD G(2,2);
  TMatrixD V(2,2);
  //  TMatrixD Id(4,4);
  
  
  Double_t inc = resMin;//fResKalman; // Space resolution
  if(inc==0) inc = 0.000001;
  Double_t v = X[2][0]*X[2][0] + X[3][0]*X[3][0];
  for(int i = 0;i<4;i++){
    for(int j =0;j<4;j++){
      if(i==j){
        if(i>1)
          Q[i][j]=theta*theta*(1-X[i][0]*X[i][0]/v);                //initialisation de Q
        else
          Q[i][j]=0;
      }
    }
  }
  for(int i = 0;i<2;i++){
    for(int j =0;j<2;j++){
      if(i==j){
        H[i][j]=1;
        V[i][j]=inc*inc;
        G[i][j]=1./inc*inc;
      }else{
        H[i][j]=0;
        V[i][j]=0;
        G[i][j]=0;
      }
    }
  }
  Ht=H;
  Ht.T();
  // G=V;
  // G.Invert();
  
  
  TMatrixD Ft(4,4);
  TMatrixD Cinv(4,4);
  TMatrixD M(2,1);
  TMatrixD Cproj(4,4);
  TMatrixD Xnew(4,1);
  TMatrixD Cnew(4,4);
  TMatrixD Cprojinv(4,4);  

  F[0][2]=(iMin-X[1-typeMin][0])*1./X[3-typeMin][0];
  F[1][3]=(iMin-X[1-typeMin][0])*1./X[3-typeMin][0];
  Xproj=F*X;
  // Kalman matrices calculations
  Ft = F;
  Ft.T();
  M[1-typeMin][0] = iMin;
  M[typeMin][0] = meanMin;
  Cproj=F*C*Ft+Q;
  Cprojinv=Cproj;
  Cprojinv.Invert();
  Cinv=Cprojinv+Ht*G*H;
  Cnew=Cinv;
  Cnew.Invert();
  Xnew=Cnew*((Cprojinv*Xproj)+(Ht*G*M));
  
  
  if(fCanvas[plane]){ // Display for Reco monitor
    Float_t x[3] = {(Float_t) (X[1-typeMin][0]),
                    (Float_t) (iMin),
                    (Float_t) (iMin)};
    Float_t y[3] = {(Float_t) (X[typeMin][0]),
                    (Float_t) (Xproj[typeMin][0]-rangeMin),
                    (Float_t) (Xproj[typeMin][0]+rangeMin)};
    if(gHarpoDebug>2)
      Info("FindNextClosest","%g %g     %g %g    %g %g",x[0],y[0],x[1],y[1],x[2],y[2]);
    TPolyLine* triangle;
    if(typeMin == TCLUSTER) triangle = new TPolyLine(3,x,y,"FL");
    else triangle = new TPolyLine(3,y,x,"FL");
    triangle->SetLineColor(kBlack);
    //triangle->SetFillColor(kRed);
    triangle->SetFillColor(color);
    triangle->SetFillStyle(fill);
    fCanvas[plane]->cd();
    triangle->Draw("FL");
    fHist[plane]->GetXaxis()->SetRangeUser(X[0][0]-20,X[0][0]+20);
    fHist[plane]->GetYaxis()->SetRangeUser(X[1][0]-20,X[1][0]+20);
    fCanvas[plane]->Update();
  }
  used[iclMin] = iTr;
  if(smooth == 1) reused[iclMin] = 1;  
  else reused[iclMin] = 0;  
  if(smooth == 2) used[iclMin] = 1000;  

  Bool_t skip = kFALSE; // If already in this track, skip
  Int_t kcl = 0;
  if(arr != 0){
    while(arr->At(kcl) != 0){
      if(arr->At(kcl) == 0) break;
      if(arr->At(kcl) == iclMin+1){
        skip = kTRUE;
        break;
      }
      kcl++;
    }
    if(!skip)
      //Info("FindNext","Add cluster %d",iclMin);
    arr->AddAt(iclMin+1,kcl);
  }
  if(gHarpoDebug>2)
    Info("FindNextClosest","Add cluster %d, (%d  %p) %d",iclMin,iTr,arr,fStartIndex[iTr]%10);
  
  if(smooth == 0){
    fStartPointZ[iTr][fStartIndex[iTr]%10] = X[0][0];
    fStartPointX[iTr][fStartIndex[iTr]%10] = X[1][0];
    fStartDirZ[iTr][fStartIndex[iTr]%10] = X[2][0];
    fStartDirX[iTr][fStartIndex[iTr]%10] = X[3][0];
    fStartIndex[iTr]++;
  }
  
  if(smooth == 2){
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(iclMin);
    cluster->SetZfit(0,Xnew[0][0]);
    cluster->SetXfit(0,Xnew[1][0]);
  }
  angle += angleMin;
  ncl++;
  if(gHarpoDebug>2)
    Info("FindNextClosest","Test %g %d %p %p %d %d %d %d %d", angle, iTr, clArray, arr, ncl, plane, color, fill,smooth);
  arr = FindNextClosest(Xnew, Cnew, angle, iTr, clArray, arr, ncl, plane, color, fill,smooth,qMin);


  return arr;
}


Double_t HarpoKalman::GetResolution(HarpoRecoClusters* cl)
{

  if((cl->GetQuality() & 3) == 3) return 2;
  if(cl->GetQuality() & 4) return 1;
  if(cl->GetQuality() & 3) return 1;//0.5;


  if(cl->GetType() == CCLUSTER) return 1;
  if(cl->GetType() == TCLUSTER) return 0.2;

  return 10;
}


 void   HarpoKalman::Init()
 {

   // Initialise histograms here
   
   fCanvas[0] = NULL;
   fCanvas[1] = NULL;
   fHist[0] = NULL;
   fHist[1] = NULL;

   fNclMin = 10;
   fNclMin2 = 15;
   fMaxSlopeType = 1.5;
   fResKalman = 0.2;
   fScatKalman = 1;
   fResFinder = 1;
   fScatFinder = 0.5;

   fQmin = 200;
   fWidthMinT = 2;
   fWidthMinC = 5;
   fWidthMax = 30;




        //   fNtr = 0;
   hNcl = new TH1F("hNcl","hNcl",500,0,500);

  const Int_t nbinsDist = 200;
  Double_t xminDist = 1;
  Double_t xmaxDist = 10000;
  Double_t logxminDist = TMath::Log(xminDist);
  Double_t logxmaxDist = TMath::Log(xmaxDist);
  Double_t binwidthDist = (logxmaxDist-logxminDist)/nbinsDist;
  Double_t xbinsDist[nbinsDist+1];
  xbinsDist[0] = xminDist;
  for (Int_t i=1;i<=nbinsDist;i++)
    xbinsDist[i] = TMath::Exp(logxminDist+i*binwidthDist);


  const Int_t nbinsTheta = 100;
  Double_t xminTheta = 1e-5;
  Double_t xmaxTheta = 2;
  Double_t logxminTheta = TMath::Log(xminTheta);
  Double_t logxmaxTheta = TMath::Log(xmaxTheta);
  Double_t binwidthTheta = (logxmaxTheta-logxminTheta)/nbinsTheta;
  Double_t xbinsTheta[nbinsTheta+1];
  xbinsTheta[0] = xminTheta;
  for (Int_t i=1;i<=nbinsTheta;i++)
    xbinsTheta[i] = TMath::Exp(logxminTheta+i*binwidthTheta);

   hDistTracks = new TH1F("hDistTracks","",nbinsDist,xbinsDist);
   hDistMinTracks = new TH1F("hDistMinTracks","",nbinsDist,xbinsDist);
   hThetaTracks = new TH1F("hThetaTracks","",nbinsTheta,xbinsTheta);//,100,-1,1);
   //   hThetaTracksNtr = new TH2F("hThetaTracksNtr","",NTRACK,0,NTRACK,100,-1,1);
   hThetaTracksNtr = new TH2F("hThetaTracksNtr","",nbinsDist,xbinsDist,nbinsTheta,xbinsTheta);
   for(Int_t i = 0; i<10; i++)
     hThetaDist[i] = new TH2F(Form("hThetaDist%d",i),"",nbinsDist,xbinsDist,nbinsTheta,xbinsTheta);

 
  const Int_t nbinsQ = 100;
  Double_t xminQ = 1e-3;
  Double_t xmaxQ = 1e3;
  Double_t logxminQ = TMath::Log(xminQ);
  Double_t logxmaxQ = TMath::Log(xmaxQ);
  Double_t binwidthQ = (logxmaxQ-logxminQ)/nbinsQ;
  Double_t xbinsQ[nbinsQ+1];
  xbinsQ[0] = xminQ;
  for (Int_t i=1;i<=nbinsQ;i++)
    xbinsQ[i] = TMath::Exp(logxminQ+i*binwidthQ);
  fHistQQtestVsDist = new TH2F("fHistQQtestVsDist","",nbinsQ,xbinsQ,nbinsDist,xbinsDist);
   fHistQQmin = new TH2F("fHistQQmin","",nbinsQ,xbinsQ,nbinsDist,xbinsDist);

 }

void   HarpoKalman::Save(char * /* mode */)
 {

   // TString * hstFile = gHConfig->GetHistFile();
   // if ( hstFile == NULL ) {
   //   std::cout << gHConfig->GetProgramName() << " " <<
   //     "No Hist File name given, use default" << std::endl;
   //   //    const char* dir = gSystem->ExpandPathName("$HARPO_ANA_DIR/reco");
   //  const char* dir = "$HARPO_ANA_DIR/reco";
   //  if(gSystem->AccessPathName(dir))
   //    hstFile = new TString(Form("trackingKalman%lli.root",gHConfig->GetRunNo()));
   //  else
   //    hstFile = new TString(Form("%s/trackingKalman%lli.root",dir,gHConfig->GetRunNo()));
   //  //    hstFile = new TString(Form("outputs/anaph%lli.root",gHConfig->GetRunNo()));
   // }

   // TFile *hf = new TFile(hstFile->Data(),"RECREATE");
   OpenHistFile("trackingKalman");
   // Save histograms here
   hDistTracks->Write();
   hDistMinTracks->Write();
   hThetaTracks->Write();
   hThetaTracksNtr->Write();
   for(Int_t i = 0; i<10; i++){
     if(hThetaDist[i])
       hThetaDist[i]->Write();
   }
   fHistQQtestVsDist->Write();
   fHistQQmin->Write();

   //   hf->Close();
   //   printf("fNewFile %s closed\n", hstFile->Data() );
 
 }