CgemSegmentFun.cxx

Go to the documentation of this file.

#include "CgemSegmentFitAlg/CgemSegmentFun.h"
 
#include "CLHEP/Units/PhysicalConstants.h"
 
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Geometry/Point3D.h"
#include "KalFitAlg/helix/Helix.h"
//#include "TrackUtil/Helix.h"
 
#include "CLHEP/Matrix/Matrix.h"
 
#include "TROOT.h"    
#include "TObjArray.h"
#include "TH1F.h"     
#include "TF1.h"     
#include "TGraph.h"  
 
#include <iostream>
#include <iomanip>
#include <cstring>
 
using namespace std;
 
/////////////////////////////////////////////////////////////////////////////
 
void CgemSegmentFun::GetChisqF(CLHEP::HepVector trkpar, HepPoint3D pivot, double& chisq){
   
   double dphi[3],dz[3];
   HepMatrix delta_Par(2,1);
   HepSymMatrix ParErr(2);
   HepMatrix S(2,2);
   HepSymMatrix HitErr(2);
 
   KalmanFit::Helix* cgem_helix = new KalmanFit::Helix(pivot, trkpar);
   chisq = 0.;
 
   for(int i = 0; i < 3; i++){
      double tmp_x   = cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).x();
      double tmp_y   = cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).y();
      double tmp_phi = atan2(tmp_y , tmp_x ); //[-pi,pi)
      dphi[i] = vec_phi[i] - tmp_phi;
      if(dphi[i] < -M_PI) dphi[i] += 2*M_PI;
      if(dphi[i] >= M_PI) dphi[i] -= 2*M_PI;
      dz[i]   = vec_z[i]   - cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).z();
//      cout<<"dphi["<<i<<"] = "<<dphi[i]<<endl;
//      cout<<"dz["<<i<<"] = "<<dz[i]<<endl;
 
      delta_Par(1,1) = dphi[i]; delta_Par(2,1) = dz[i];
 
      ParErr(1,1) = x_reso[i]*x_reso[i]; ParErr(2,2) = v_reso[i]*v_reso[i]; ParErr(1,2) = 0.; ParErr(2,1) = 0.;
      S(1,1) = 1./r_layer[i]; S(1,2) = 0.; S(2,1) = -1./tan(a_stero[i]); S(2,2) = 1/sin(a_stero[i]); // which radius?
//      HitErr = ParErr.similarity(S);
//      int& ifail = i;
//      HitErr.invert(ifail);
      HitErr(1,1) = (pow(S(2,2),2)*ParErr(2,2)+pow(S(2,1),2)*ParErr(1,1))/(pow(S(2,2)*S(1,1),2)*ParErr(2,2)*ParErr(1,1)); 
      HitErr(1,2) = -S(2,1)/(S(1,1)*ParErr(2,2)*pow(S(2,2),2)); 
      HitErr(2,1) = -S(2,1)/(S(1,1)*ParErr(2,2)*pow(S(2,2),2)); 
      HitErr(2,2) = 1./(ParErr(2,2)*pow(S(2,2),2)); 
 
      chisq += (HitErr.similarityT(delta_Par)).determinant();
//      double sigma = (HitErr.similarityT(delta_Par)).determinant();
//      chisq += (delta_Par.T()*delta_Par).determinant()/sigma;
   }
   delete cgem_helix;
}
 
HepSymMatrix CgemSegmentFun::InvC(int i)
{
    HepSymMatrix C(2,0);
    C(1,1) = (r_X[i]*r_X[i]*v_reso[i]*v_reso[i]+cos(a_stero[i])*cos(a_stero[i])*r_V[i]*r_V[i]*x_reso[i]*x_reso[i])/(rl[i]*rl[i]*x_reso[i]*x_reso[i]*v_reso[i]*v_reso[i]);
    C(2,1) = r_V[i]*sin(a_stero[i])*cos(a_stero[i])/(rl[i]*v_reso[i]*v_reso[i]);
    C(2,2) = sin(a_stero[i])*sin(a_stero[i])/(v_reso[i]*v_reso[i]);
    return C;
}
 
HepVector CgemSegmentFun::CalculateHelix(CLHEP::HepVector trkpar, HepPoint3D pivot)
{
    KalmanFit::Helix cgem_helix_0(pivot, trkpar);
    HepMatrix J_dmdx(2,3,0), J_dxda(3,5,0), J(2,5,0), A(6,5,0);
    HepSymMatrix U(5, 0), W(6,0), C(2,0);
    HepVector a_new(5), dm(6), da(5);
    double dphi[3],dz[3];
    
    for(int i = 0; i < 3; i++) {
        double d_phi = IntersectCylinder(rl[i], cgem_helix_0);
        HepPoint3D pos = cgem_helix_0.x(d_phi);
        double x = pos.x();
        double y = pos.y();
        double z = pos.z();
        double r2 = x*x+y*y;
        double tmp_phi = atan2(y, x); //[-pi,pi)
        dphi[i] = vec_phi[i] - tmp_phi;
        if(dphi[i] < -M_PI) dphi[i] += 2*M_PI;
        if(dphi[i] >= M_PI) dphi[i] -= 2*M_PI;
        
        dm[i*2]=dphi[i]*rl[i];
        dm[i*2+1]=vec_z[i]-z;
        //cout<<"rl, r ="<<rl[i]<<", "<<sqrt(r2)<<endl;
        
        J_dmdx(1,1)=-y/rl[i];
        J_dmdx(1,2)= x/rl[i];
        J_dmdx(2,3)= 1;
        J_dxda=cgem_helix_0.delXDelA(d_phi);
        J=J_dmdx*J_dxda;
        A.sub(i*2+1,1,J);
        
        C=InvC(i);
        W.sub(i*2+1,C);
        
        cout<<"iLayer = "<<i<<endl;
        cout<<"dm: "<<dm<<endl;
        cout<<"J_dmdx: "<<J_dmdx<<endl;
        cout<<"J_dxda: "<<J_dxda<<endl;
        cout<<"J: "<<J<<endl;
        cout<<"A: "<<A<<endl;
        cout<<"C: "<<C<<endl;
        cout<<"W: "<<W<<endl;
    }
    int ifail=99;
    U=W.similarityT(A);
    cout<<"U = "<<U<<endl;
    cout<<"U.det() = "<<U.determinant()<<endl;
    double det=0.;
    if(DetSquareMatrix(U,det)) cout<<"det = "<<det<<endl;
    HepSymMatrix U_inv = U;
    if(InverseSymMatrix(U,U_inv)) {
        cout<<"U_inv = "<<U_inv<<endl;
        cout<<"I = "<<U*U_inv<<endl;
    }
    
    //HepMatrix UU=A.T()*W*A;
    //cout<<"UU = "<<UU<<endl;
    HepSymMatrix U0 = U;
    
    HepSymMatrix U1=U;
    cout<<"U1 = "<<U1<<endl;
    U1.invertCholesky5(ifail);
    cout<<"ifail = "<<ifail<<endl;
    cout<<"U1^-1 = "<<U1<<endl;
    cout<<"I1="<<U*U1<<endl;
    
    HepSymMatrix U2=U;
    cout<<"U2 = "<<U2<<endl;
    U2.invertHaywood5(ifail);
    cout<<"ifail = "<<ifail<<endl;
    cout<<"U2^-1 = "<<U2<<endl;
    cout<<"I2="<<U*U2<<endl;
 
    HepSymMatrix U3=U;
    cout<<"U3 = "<<U3<<endl;
    U3.invertBunchKaufman(ifail);
    cout<<"ifail = "<<ifail<<endl;
    cout<<"U3^-1 = "<<U3<<endl;
    cout<<"I3="<<U*U3<<endl;
    
////////HepSymMatrix U4(5,1);
////////cout<<"U4 = "<<U4<<endl;
////////U4.invertBunchKaufman(ifail);
////////cout<<"ifail = "<<ifail<<endl;
////////cout<<"U4^-1 = "<<U4<<endl;
    
    U.invert(ifail);
    cout<<"ifail = "<<ifail<<endl;
    cout<<"U^-1 = "<<U<<endl;
    cout<<"I="<<U*U0<<endl;
    da=U*A.T()*W*dm;
    //cout<<"A="<<A<<endl;
    //cout<<"A^T="<<A.T()<<endl;
    cout<<"del_a="<<da<<endl;
    a_new=trkpar+da;
    cout<<"a_new="<<a_new<<endl;
    return a_new;
}
 
void CgemSegmentFun::GetLikelihoodF(CLHEP::HepVector trkpar, HepPoint3D pivot, double& chisq){
 
   double dphi[3],dv[3];
   double total = 1.;
   KalmanFit::Helix* cgem_helix = new KalmanFit::Helix(pivot, trkpar);
   for(int i = 0; i < 3; i++){
      double tmp_x   = cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).x();
      double tmp_y   = cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).y();
      double tmp_z   = cgem_helix->x(IntersectCylinder(rl[i], *cgem_helix)).z();
      double tmp_phi = atan2(tmp_y , tmp_x ); //[-pi,pi)
      while(tmp_phi<0) tmp_phi +=  2*M_PI;
      dphi[i] = tmp_phi - vec_phi[i];
      if(dphi[i] < -M_PI) dphi[i] += 2*M_PI;
      if(dphi[i] >= M_PI) dphi[i] -= 2*M_PI;
 
      double tmp_v = tmp_z*sin(a_stero[i])*r_layer[i]/rl[i]+r_layer[i]*tmp_phi*cos(a_stero[i]);
      dv[i] = tmp_v - vec_v[i];
   
      double f1;
      if(nStrip_X[i] == 1) f1 = fLadder(dphi[i], DeltaPhi[i]);
      else f1 = fGauss(dphi[i], phi_mean[i], phi_reso[i]);
//      f1 = fGauss(dphi[i], phi_mean[i], phi_reso[i]);
      double f2 = fGauss(dv[i], v_mean[i], v_reso[i]);
    total *= f1*f2;
   }
   chisq = -1.*log(total);
   delete cgem_helix;
}
 
void CgemSegmentFun::fcnTrk(int &npar, double *gin, double &f, double *par, int iflag){
   
   HepPoint3D pivot(0.,0.,0.);
   HepVector trkpar(5);
   for(int i = 0; i < 5; i++){
      trkpar[i] = par[i];
   }
   GetChisqF(trkpar, pivot, f);
//   GetLikelihoodF(trkpar, pivot, f);
}
 
double CgemSegmentFun::fLadder(double x, double delta_phi){
   if(fabs(x)>delta_phi) return 0.;
   else return 0.5/delta_phi;
}
 
double CgemSegmentFun::fGauss(double x, double mean, double sigma){
   return 1./sqrt(2*M_PI)/sigma*exp(-1.*pow((x-mean)/sigma,2)/2);
}
 
double CgemSegmentFun::IntersectCylinder(double r, KalmanFit::Helix helix){
   double m_rad = helix.radius();
   double l = helix.center().perp();
 
   double cosPhi = (m_rad * m_rad + l * l  - r * r) / (2 * m_rad * l);
 
   if(cosPhi < -1 || cosPhi > 1) return 0;
 
   double dPhi = helix.center().phi() - acos(cosPhi) - helix.phi0();
 
   if(dPhi < -M_PI) dPhi += 2 * M_PI;
 
   return dPhi;
}
 
double CgemSegmentFun::GetKappaAfterFit(int charge, double* rec_phi){
   
   double x[3]={0.};double y[3]= {0.};
   for(int i = 0; i < 3; i++){
      x[i] = r_layer[i]*cos(rec_phi[i]); //get the position on the anode
      y[i] = r_layer[i]*sin(rec_phi[i]);
   }
   double x0,y0,r;
   x0 = ((pow(x[2],2)+pow(y[2],2))*(y[1]-y[0])+(pow(x[1],2)+pow(y[1],2))*(y[0]-y[2])+(pow(x[0],2)+pow(y[0],2))*(y[2]-y[1]))/(2.*((x[2] - x[0])*(y[1] - y[0]) - (x[1] -x[0])*(y[2] - y[0])));
   y0 = ((pow(x[2],2)+pow(y[2],2))*(x[1]-x[0])+(pow(x[1],2)+pow(y[1],2))*(x[0]-x[2])+(pow(x[0],2)+pow(y[0],2))*(x[2]-x[1]))/(2.*((y[2] - y[0])*(x[1] - x[0]) - (y[1] -y[0])*(x[2] - x[0])));
   r = sqrt(pow(x[1]-x0, 2) + pow(y[1]-y0,2));
 
   return 333.564*charge/r;
}
 
void CgemSegmentFun::GetHelixVarBeforeFit(int charge, double& d0, double& phi0, double& a_kappa, double& z0, double& tanl){
 
   double x[3]={0.};double y[3]= {0.};
   for(int i = 0; i < 3; i++){
      x[i] = rl[i]*cos(vec_phi[i]);
      y[i] = rl[i]*sin(vec_phi[i]);
   }
   double x0,y0,r;
   x0 = ((pow(x[2],2)+pow(y[2],2))*(y[1]-y[0])+(pow(x[1],2)+pow(y[1],2))*(y[0]-y[2])+(pow(x[0],2)+pow(y[0],2))*(y[2]-y[1]))/(2.*((x[2] - x[0])*(y[1] - y[0]) - (x[1] -x[0])*(y[2] - y[0])));
   y0 = ((pow(x[2],2)+pow(y[2],2))*(x[1]-x[0])+(pow(x[1],2)+pow(y[1],2))*(x[0]-x[2])+(pow(x[0],2)+pow(y[0],2))*(x[2]-x[1]))/(2.*((y[2] - y[0])*(x[1] - x[0]) - (y[1] -y[0])*(x[2] - x[0])));
 
 
   r = sqrt(pow(x[1]-x0, 2) + pow(y[1]-y0,2));
//   cout<<"r = "<<r<<endl;
//   cout<<"x0 = "<<x0<<"\ty0 = "<<y0<<endl;
   d0 = sqrt(x0*x0 + y0*y0) - r;
   if(d0 >= 0) d0 =  charge*d0;
   else        d0 = -charge*d0;
   //phi0 = atan2(y0,x0) + M_PI/2.;
   phi0 = atan2(-y0*charge,-x0*charge);
   //phi0 = atan2(y0*charge,x0*charge);
   //phi0 = atan2(y0,x0);
   if(phi0 < 0)phi0 += 2.*M_PI;
   //   omega = 1/r;
   double  pt_temp= r/333.564*charge;
   a_kappa = 1./pt_temp;
 
   TGraph* Zz = new TGraph(3);
   TF1* f1 = new TF1("f1","[0]+[1]*x",-1000.,1000.);
   double s[3];
   for(int i = 0; i < 3; i++){
      s[i] = r*acos(((x0 - x[i])*x0 + (y0 - y[i])*y0)/r/sqrt(x0*x0 + y0*y0));
      Zz->SetPoint(i,s[i],vec_z[i]); 
      //cout<<"s, z = "<<s[i]<<", "<<vec_z[i]<<"; cosAng = "<<((x0 - x[i])*x0 + (y0 - y[i])*y0)/r/sqrt(x0*x0 + y0*y0)<<", phi0 = "<<phi0<<endl;
   }
   Zz->Fit("f1","Q");
   z0 = f1->GetParameter(0);
   tanl = f1->GetParameter(1);
   delete f1;
   delete Zz;
 
   //std::cout<<" Get Helix Var before global fit  PatPar :"<<d0<<"\t"<<phi0<<"\t"<<a_kappa<<"\t"<<z0<<"\t"<<tanl<<  std::endl;
}
 
double CgemSegmentFun::detA(double arcs[10][10],int n)
{
    if(n==1)
    {
        return arcs[0][0];
    }
    double ans = 0;
    double temp[10][10]={0.0};
    int i,j,k;
    for(i=0;i<n;i++)
    {
        for(j=0;j<n-1;j++)
        {
            for(k=0;k<n-1;k++)
            {
                temp[j][k] = arcs[j+1][(k>=i)?k+1:k];
 
            }
        }
        double t = detA(temp,n-1);
        if(i%2==0)
        {
            ans += arcs[0][i]*t;
        }
        else
        {
            ans -=  arcs[0][i]*t;
        }
    }
    return ans;
}
 
bool CgemSegmentFun::DetSquareMatrix(HepMatrix m, double &det)
{
    det = 0.;
    cout<<"m="<<m<<endl;
    int nrow=m.num_row();
    int ncol=m.num_col();
    if(nrow!=ncol||nrow<=0||ncol<=0) return false;
 
    int n = nrow;
    double arcs[10][10];
    for(int i=0; i<n; i++)
        for(int j=0; j<n; j++)
            arcs[i][j]=m(i+1,j+1);
    det = detA(arcs,n);
    return true;
}
 
bool CgemSegmentFun::GetMatrixInverse(double src[10][10],int n,double des[10][10])
{
    double flag=detA(src,n);
    double t[10][10];
    if(flag==0)
    {
        return false;
    }
    else
    {
        getAStart(src,n,t);
        for(int i=0;i<n;i++)
        {
            for(int j=0;j<n;j++)
            {
                des[i][j]=t[i][j]/flag;
            }
 
        }
    }
 
 
    return true;
 
}
 
void  CgemSegmentFun::getAStart(double arcs[10][10],int n,double ans[10][10])
{
    if(n==1)
    {
        ans[0][0] = 1;
        return;
    }
    int i,j,k,t;
    double temp[10][10];
    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
        {
            for(k=0;k<n-1;k++)
            {
                for(t=0;t<n-1;t++)
                {
                    temp[k][t] = arcs[k>=i?k+1:k][t>=j?t+1:t];
                }
            }
 
 
            ans[j][i]  =  detA(temp,n-1);
            if((i+j)%2 == 1)
            {
                ans[j][i] = - ans[j][i];
            }
        }
    }
}
 
bool CgemSegmentFun::InverseSymMatrix(HepSymMatrix m, HepSymMatrix &m_inv)
{
    HepMatrix mm=m;
    int n = mm.num_row();
    double arcs[10][10];
    for(int i=0; i<n; i++)
        for(int j=0; j<n; j++)
            arcs[i][j]=mm(i+1,j+1);
    double des[10][10];
    bool stat = GetMatrixInverse(arcs,n,des);
    if(stat) {
        for(int i=0; i<n; i++)
            for(int j=0; j<=i; j++)
                m_inv(i+1,j+1)=des[i][j];
    }
    return stat;
}
 
/*global variables of CGEM Geomtry*/
double CgemSegmentFun::rl[3]     = {79.838/10. ,125.104/10. ,167.604/10.}; //cm
double CgemSegmentFun::a_stero[3]= {(45.94*3.1415926/180),(31.10*3.1415926/180),(32.99*3.1415926/180)};
double CgemSegmentFun::r_layer[3]= {87.5026/10.,132.7686/10.,175.2686/10.}; //cm
double CgemSegmentFun::r_X[3];
double CgemSegmentFun::r_V[3];
double CgemSegmentFun::x_reso[3] = {0.1372/10.,0.1476/10.,0.1412/10.}; //cm
double CgemSegmentFun::v_reso[3] = {0.1273/10.,0.1326/10.,0.1378/10.}; //cm
 
double CgemSegmentFun::phi_reso[3] = {0.000371, 0.000214, 0.000165}; //'
double CgemSegmentFun::phi_mean[3] = {0.000009, 0.000003,-0.000002}; //'
//double CgemSegmentFun::phi_mean[3] = {0., 0., 0.}; //'
//double CgemSegmentFun::v_reso[3] = { 0.013702, 0.013136, 0.012774}; //cm
double CgemSegmentFun::v_mean[3] = {-0.000208, 0.000465,-0.000598}; //cm
//double CgemSegmentFun::v_mean[3] = {0., 0., 0.}; //cm
double CgemSegmentFun::DeltaPhi[3] = {0.0033, 0.0022, 0.0016}; //'
 
/*global variables of CGEM segment*/
double CgemSegmentFun::vec_phi[3];
double CgemSegmentFun::vec_z[3];
double CgemSegmentFun::vec_v[3];
int CgemSegmentFun::nStrip_X[3];
int CgemSegmentFun::nStrip_V[3];