HarpoKalmanFitting.cxx

Go to the documentation of this file.

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

#include "TEllipse.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(HarpoKalmanFitting);

void   HarpoKalmanFitting::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();
   }
 }

void   HarpoKalmanFitting::process()
{
  //  Bool_t drawEvent = kFALSE;
  nEvents++;
  if(gHarpoDebug>0)
    Info("process","Event %ld",nEvents);

  for(Int_t i = 0; i< NTRACK; i++)
    fNclTrack[i] = 0;
  if(!fEvt->GetRecoEvent())  // Reconstructed data (clusters, tracks, matched tracks)
    return;
  HarpoRecoEvent* reco = fEvt->GetRecoEvent();
  reco->SetTrackType(KALMAN);
  // Clusters in the event (both X and Y directions)
  TClonesArray* clArray = reco->GetClustersArray();
  //  Int_t nCl = clArray->GetEntries();
  TClonesArray* trArray = reco->GetKalmanTracksArray();
  Int_t nTr = trArray->GetEntries();
  Info("process","*********** %d **********",nTr);

  for(Int_t plane = 0; plane<2; plane++){
    InitPlane(clArray, plane);
    for(Int_t itr = 0; itr<nTr; itr++){
      HarpoRecoKalmanTracks* track = (HarpoRecoKalmanTracks*)trArray->At(itr);
      if(track->GetPlane() != plane) continue;
      FitTrack(clArray,plane,track);
    }
  }
}

Int_t  HarpoKalmanFitting::InitPlane(TClonesArray* clArray, Int_t plane)
{
 
  HarpoKalman::InitPlane(clArray,plane);
  for(Int_t itr = 0; itr<NTRACK; itr++){
    fId[itr][0] = -1;
    fId[itr][1] = -1;
  }

  return 1;
}



TMatrixD HarpoKalmanFitting::GetTrackEnd(TClonesArray* clArray, Int_t plane, Int_t iTr)
{
  
  Int_t iMin = NCHAN+1, iMax = -1, jMin = NADC+1, jMax = -1;
  Double_t xMax = 0, zMax = 0;
  Int_t nCl = clArray->GetEntries();
  for(Int_t icl = 0; icl<nCl; icl++){
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    Int_t index = cluster->GetIndex();
    Int_t mean  = cluster->GetMean();
    if(cluster->GetIdClusterTrack() != iTr) continue;
    if(cluster->GetPlane() != plane) continue;
    if(cluster->GetType() == TCLUSTER){
      if(index>jMax){ jMax = index; xMax = mean;}
      if(index<jMin) jMin = index;
    }
    if(cluster->GetType() == CCLUSTER){
      if(index>iMax){ iMax = index; zMax = mean;}
      if(index<iMin) iMin = index;
    }
  }
  TMatrixD X(4,1);
  if(iMax-iMin > jMax-jMin){ // vertical track
    X[0][0] = zMax;
    X[1][0] = iMax;
    X[2][0] = 0;
    X[3][0] = -1;
  }else{                     // horizontal track
    X[0][0] = jMax;
    X[1][0] = xMax;
    X[2][0] = -1;
    X[3][0] = 0;
  }

  return X;
}




Int_t HarpoKalmanFitting::FitTrack(TClonesArray* clArray, Int_t plane, HarpoRecoKalmanTracks* track)
{

  Int_t iTr = track->GetNtrack();

  // TMatrixD X(4,1);
  // X[0][0] = track->GetZend();
  // X[1][0] = track->GetXend();
  // X[2][0] = track->GetZstart() - track->GetZend();
  // X[3][0] = track->GetZstart() - track->GetZend();
 
  TMatrixD X = GetTrackEnd(clArray,plane,iTr);

  // if(gHarpoDebug>1) 
    Info("FitTrack","New seed: (%g, %g, %g, %g)",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;
    }
  }  
  TArrayI* arr = new TArrayI(2000);
  Double_t angleorig = TMath::ATan2(X[2][0],X[3][0]);
  //##################################################################################
  arr = FindNextClosest(X,Corig,angleorig,iTr,clArray,arr,1,plane,iTr+2,3002,2,-1);//#
  //##################################################################################

  Int_t n = 0;
  for(Int_t i = 0; i<2000; i++){
    if(arr->At(i)-1<0) break;
    n++;
  }
  Info("FitTrack","NclOrig = %d, NclFit = %d",track->GetNclusterTrack(),n);
  if(n<fNclMin){
    delete arr;
    return -n;
  }


  Int_t offset = 10;
  HarpoRecoClusters* clusterSt1 =  (HarpoRecoClusters*)clArray->At(arr->At(offset)-1);
  HarpoRecoClusters* clusterSt2 =  (HarpoRecoClusters*)clArray->At(arr->At(offset + 1)-1);
  Double_t xSt = clusterSt1->GetXfit();
  Double_t zSt = clusterSt1->GetZfit();
  Double_t pxSt = clusterSt2->GetXfit() - clusterSt1->GetXfit();
  Double_t pzSt = clusterSt2->GetZfit() - clusterSt1->GetZfit();
  HarpoRecoClusters* clusterEnd1 =  (HarpoRecoClusters*)clArray->At(arr->At(n-offset)-1);
  HarpoRecoClusters* clusterEnd2 =  (HarpoRecoClusters*)clArray->At(arr->At(n-offset - 1)-1);
  Double_t xEnd = clusterEnd1->GetXfit();
  Double_t zEnd = clusterEnd1->GetZfit();
  Double_t pxEnd = clusterEnd2->GetXfit() - clusterEnd1->GetXfit();
  Double_t pzEnd = clusterEnd2->GetZfit() - clusterEnd1->GetZfit();


  track->SetPxStart(pxSt);
  track->SetPzStart(pzSt);
  track->SetPxEnd(pxEnd);
  track->SetPzEnd(pzEnd);

  track->SetXstart(xSt);
  track->SetZstart(zSt);
  track->SetXend(xEnd);
  track->SetZend(zEnd);
 
  
  if(fCanvas[plane]){ // Display for Reco monitor
    // TEllipse* pStart = new TEllipse();
    // pStart->SetFillColor(iTr+2);
    // pStart->DrawEllipse(zSt,xSt,5,0,0,360,0);
    // fHist[plane]->GetXaxis()->SetRangeUser(zSt-20,zSt+20);
    // fHist[plane]->GetYaxis()->SetRangeUser(xSt-20,xSt+20);
    // fCanvas[plane]->Update();
    // gSystem->Sleep(200);
    // TEllipse* pEnd = new TEllipse();
    // pEnd->SetFillColor(iTr+2);
    // pEnd->SetFillStyle(3002);
    // pEnd->DrawEllipse(zEnd,xEnd,5,0,0,360,0);
    // fHist[plane]->GetXaxis()->SetRangeUser(zEnd-20,zEnd+20);
    // fHist[plane]->GetYaxis()->SetRangeUser(xEnd-20,xEnd+20);
    // fCanvas[plane]->Update();
    // gSystem->Sleep(200);



    TArrow* arrowSt = new TArrow();
    arrowSt->SetLineColor(iTr+2);
    arrowSt->SetFillColor(iTr+2);
    fCanvas[plane]->cd();
    arrowSt->DrawArrow(zSt,xSt,zSt+10*pzSt,xSt+10*pxSt);
    fHist[plane]->GetXaxis()->SetRangeUser(zSt-20,zSt+20);
    fHist[plane]->GetYaxis()->SetRangeUser(xSt-20,xSt+20);
    fCanvas[plane]->Update();
    gSystem->Sleep(2000);
    TArrow* arrowEnd = new TArrow();
    arrowEnd->SetLineColor(iTr+2);
    arrowEnd->SetFillColor(iTr+2);
    fCanvas[plane]->cd();
    arrowEnd->DrawArrow(zEnd,xEnd,zEnd+10*pzEnd,xEnd+10*pxEnd);
    fHist[plane]->GetXaxis()->SetRangeUser(zEnd-20,zEnd+20);
    fHist[plane]->GetYaxis()->SetRangeUser(xEnd-20,xEnd+20);
    fCanvas[plane]->Update();
    gSystem->Sleep(2000);
  }

  delete arr;

  return n;
}




Double_t HarpoKalmanFitting::GetQtrack(Int_t itr, Int_t plane)
{
  HarpoDccMap* map = fEvt->GetDccMap(plane);

  HarpoRecoEvent* reco = fEvt->GetRecoEvent();
  TClonesArray* clArray = reco->GetClustersArray();
  Int_t nCl = clArray->GetEntries();
  Double_t qTr = 0;
  for(Int_t icl = 0; icl<nCl; icl++){
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    if(cluster->GetPlane() != plane) continue;
    if(!cluster->CheckIdClusterTrack(itr)) continue;
    Int_t type = cluster->GetType();
    Int_t ind = cluster->GetIndex();
    Int_t ind2 = cluster->GetIfirst();
    Int_t width = cluster->GetWidth();
    for(Int_t k = ind2; k< ind2 + width; k++){
      Int_t i = -1, j = -1;
      if(type == CCLUSTER){ i = ind; j = k;}
      if(type == TCLUSTER){ i = k; j = ind;}
      if(i<0 || i>=NCHAN) continue;
      if(j<0 || j>=NADC) continue;
      Double_t q = map->GetData(i,j);
      //      Double_t q = fMapTmp[i][j];
      if(q<-500) continue;
      ULong64_t test = 1 << itr; 
      //Info("GetQtrack","TEST = %llX   |   %llX  =>  %llX,  %llX",test,fMapTmp[i][j],fMapTmp[i][j]&test,fMapTmp[i][j]|test);
      if(fMapTmp[i][j] & test) continue;
      qTr += q;
      // fMapTmp[i][j] = -1000;
      fMapTmp[i][j] |= test;
    }
  }
  if(gHarpoDebug>0)
    Info("GetQtrack","Plane %d, Track %d: Q = %g/%g",plane,itr,qTr,fQtot);
  return qTr;
}

Double_t HarpoKalmanFitting::GetQtracks(Int_t plane)
{
  HarpoDccMap* map = fEvt->GetDccMap(plane);

  HarpoRecoEvent* reco = fEvt->GetRecoEvent();
  TClonesArray* clArray = reco->GetClustersArray();
  Int_t nCl = clArray->GetEntries();
  //  Double_t qTr = 0;

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

  for(Int_t icl = 0; icl<nCl; icl++){
    HarpoRecoClusters* cluster = (HarpoRecoClusters*)clArray->At(icl);
    if(cluster->GetPlane() != plane) continue;
    Int_t type = cluster->GetType();
    Int_t ind = cluster->GetIndex();
    Int_t ind2 = cluster->GetIfirst();
    Int_t width = cluster->GetWidth();
    Int_t nTr = cluster->GetNtr();
    for(Int_t iTr = 0; iTr<nTr;iTr++){
      Int_t itr = cluster->GetIdClusterTrack(iTr);
      for(Int_t k = ind2; k< ind2 + width; k++){
        Int_t i = -1, j = -1;
        if(type == CCLUSTER){ i = ind; j = k;}
        if(type == TCLUSTER){ i = k; j = ind;}
        if(i<0 || i>=NCHAN) continue;
        if(j<0 || j>=NADC) continue;
        Double_t q = map->GetData(i,j);
        //      Double_t q = fMapTmp[i][j];
        if(q<-500) continue;
        ULong64_t test = 1 << itr; 
        //Info("GetQtrack","TEST = %llX   |   %llX  =>  %llX,  %llX",test,fMapTmp[i][j],fMapTmp[i][j]&test,fMapTmp[i][j]|test);
        if(used4Q[i][j] & test) continue;
        //      qTr += q;
        // fMapTmp[i][j] = -1000;
        used4Q[i][j] |= test;
      }
    }
  }

  //  Double_t qTr[fNtr[plane]][fNtr[plane]];
  for(Int_t i = 0; i<fNtr[plane]; i++){
    for(Int_t j = 0; j<fNtr[plane]; j++){
      fQcommon[i][j] = 0;
    }
  }
  for(Int_t i = 0; i<NCHAN; i++){
    for(Int_t j = 0; j<NADC; j++){
      Double_t q = map->GetData(i,j);
      if(q<-500) continue;

      for(Int_t itr1 = 0; itr1<fNtr[plane]; itr1++){
        ULong64_t test1 = 1 << itr1;
        if(!(used4Q[i][j] & test1)) continue;
        for(Int_t itr2 = itr1; itr2<fNtr[plane]; itr2++){
          ULong64_t test2 = 1 << itr2;
          if(!(used4Q[i][j] & test2)) continue;
          fQcommon[itr1][itr2] += q;
        }
      }
    }
  }

  if(gHarpoDebug>1){
    for(Int_t i = 0; i<fNtr[plane]; i++){
      for(Int_t j = 0; j<fNtr[plane]; j++){
        std::cout << fQcommon[i][j] << " ";
      }
      std::cout << std::endl;
    }
  }

  //  Info("GetQtrack","Plane %d, Track %d: Q = %g/%g",plane,itr,qTr,fQtot);
  return 0;
}


void   HarpoKalmanFitting::Init()
 {
   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);
   
   // for(Int_t itr = 0; itr<NTRACK; itr++){
   //   fGraphs[0][itr] = 0;
   //   fGraphs[1][itr] = 0;
   // }


  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   HarpoKalmanFitting::Save(char * /* mode */)
 {

   OpenHistFile("recoKalmanFitting");
   // 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();

 }