Changeset 0b8551f in git for external/TrackCovariance/TrkUtil.cc

Timestamp:

Nov 29, 2021, 4:04:38 PM (3 years ago)

Author:

GitHub <noreply@…>

Branches:

master

Children:

0c0c9af

Parents:

9a7ea36 (diff), bd376e3 (diff)
Note: this is a merge changeset, the changes displayed below correspond to the merge itself.
Use the (diff) links above to see all the changes relative to each parent.

git-author:

Michele Selvaggi <michele.selvaggi@…> (11/29/21 16:04:38)

git-committer:

GitHub <noreply@…> (11/29/21 16:04:38)

Message:

Merge pull request #102 from fbedesch/master

Major update to handle highly displaced tracks

File:

• external/TrackCovariance/TrkUtil.cc (modified) (5 diffs)

external/TrackCovariance/TrkUtil.cc

@@ -3 +3 @@
 #include <algorithm>
 #include <TSpline.h>
+#include <TDecompChol.h>
 
 // Constructor
@@ -33 +34 @@
         fZmin = 0.0;                            // Lower                DCH z
         fZmax = 0.0;                            // Higher       DCH z
+}
+//
+// Distance between two lines
+//
+void TrkUtil::LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance)
+{
+        TMatrixDSym M(2);
+        M(0,0) = dirx.Mag2();
+        M(1,1) = diry.Mag2();
+        M(0,1) = -dirx.Dot(diry);
+        M(1,0) = M(0,1);
+        M.Invert();
+        TVectorD c(2);
+        c(0) = dirx.Dot(y0-x0);
+        c(1) = diry.Dot(x0-y0);
+        TVectorD st = M*c;
+        //
+        // Fill output
+        sx = st(0);
+        sy = st(1);
+        //
+        TVector3 x = x0+sx*dirx;
+        TVector3 y = y0+sy*diry;
+        TVector3 d = x-y;
+        distance = d.Mag();
+}
+//
+// Covariance smearing
+//
+TVectorD TrkUtil::CovSmear(TVectorD x, TMatrixDSym C)
+{
+        //
+        // Check arrays
+        //
+        // Consistency of dimensions
+        Int_t Nvec = x.GetNrows();
+        Int_t Nmat = C.GetNrows();
+        if (Nvec != Nmat || Nvec == 0)
+        {
+                std::cout << "TrkUtil::CovSmear: vector/matrix mismatch. Aborting." << std::endl;
+                exit(EXIT_FAILURE);
+        }
+        // Positive diagonal elements
+        for (Int_t i = 0; i < Nvec; i++)
+        {
+                if (C(i, i) <= 0.0)
+                {
+                        std::cout << "TrkUtil::CovSmear: covariance matrix has negative diagonal elements. Aborting." << std::endl;
+                        exit(EXIT_FAILURE);
+                }
+        }
+        //
+        // Do a Choleski decomposition and random number extraction, with appropriate stabilization
+        //
+        TMatrixDSym CvN = C;
+        TMatrixDSym DCv(Nvec); DCv.Zero();
+        TMatrixDSym DCvInv(Nvec); DCvInv.Zero();
+        for (Int_t id = 0; id < Nvec; id++)
+        {
+                Double_t dVal = TMath::Sqrt(C(id, id));
+                DCv(id, id) = dVal;
+                DCvInv(id, id) = 1.0 / dVal;
+        }
+        CvN.Similarity(DCvInv);                 // Normalize diagonal to 1
+        TDecompChol Chl(CvN);
+        Bool_t OK = Chl.Decompose();            // Choleski decomposition of normalized matrix
+        if (!OK)
+        {
+                std::cout << "TrkUtil::CovSmear: covariance matrix is not positive definite. Aborting." << std::endl;
+                exit(EXIT_FAILURE);
+        }
+        TMatrixD U = Chl.GetU();                        // Get Upper triangular matrix
+        TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
+        TVectorD r(Nvec);
+        for (Int_t i = 0; i < Nvec; i++)r(i) = gRandom->Gaus(0.0, 1.0);         // Array of normal random numbers
+        TVectorD xOut = x + DCv * (Ut * r);     // Observed parameter vector
+        //
+        return xOut;
 }
 //
@@ -48 +127 @@
         Double_t r2 = x(0) * x(0) + x(1) * x(1);
         Double_t cross = x(0) * p(1) - x(1) * p(0);
-        Double_t T = sqrt(pt * pt - 2 * a * cross + a * a * r2);
-        Double_t phi0 = atan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);    // Phi0
+        Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
+        Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);     // Phi0
         Double_t D;                                                     // Impact parameter D
         if (pt < 10.0) D = (T - pt) / a;
@@ -58 +137 @@
         Par(2) = C;             // Store C
         //Longitudinal parameters
-        Double_t B = C * sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
-        Double_t st = asin(B) / C;
+        Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
+        Double_t st = TMath::ASin(B) / C;
         Double_t ct = p(2) / pt;
         Double_t z0;
@@ -235 +314 @@
         //
         return Cmm;
+}//
+// Regularized symmetric matrix inversion
+//
+TMatrixDSym TrkUtil::RegInv(TMatrixDSym& Min)
+{
+        TMatrixDSym M = Min;                            // Decouple from input
+        Int_t N = M.GetNrows();                 // Matrix size
+        TMatrixDSym D(N); D.Zero();             // Normaliztion matrix
+        TMatrixDSym R(N);                               // Normarized matrix
+        TMatrixDSym Rinv(N);                            // Inverse of R
+        TMatrixDSym Minv(N);                            // Inverse of M
+        //
+        // Check for 0's and normalize
+        for (Int_t i = 0; i < N; i++)
+        {
+                if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
+                else D(i, i) = 1.0;
+        }
+        R = M.Similarity(D);
+        //
+        // Recursive algorithms stops when N = 2
+        //
+        //****************
+        // case N = 2  ***
+        //****************
+        if (N == 2)
+        {
+                Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
+                if (det == 0)
+                {
+                        std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
+                        Rinv.Zero();    // Return null matrix
+                }
+                else
+                {
+                        // invert matrix 
+                        Rinv(0, 0) = R(1, 1);
+                        Rinv(0, 1) = -R(0, 1);
+                        Rinv(1, 0) = Rinv(0, 1);
+                        Rinv(1, 1) = R(0, 0);
+                        Rinv *= 1. / det;
+                }
+        }
+        //****************
+        // case N > 2  ***
+        //****************
+        else
+        {
+                // Break up matrix
+                TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);   // Upper left 
+                TVectorD p(N - 1);
+                for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
+                Double_t q = R(N - 1, N - 1);
+                //Invert pieces and re-assemble
+                TMatrixDSym Ainv(N - 1);
+                TMatrixDSym A(N - 1);
+                if (TMath::Abs(q) > 1.0e-15)
+                {
+                        // Case |q| > 0
+                        Ainv.Rank1Update(p, -1.0 / q);
+                        Ainv += Q;
+                        A = RegInv(Ainv);               // Recursive call
+                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
+                        //
+                        TVectorD b = (-1.0 / q) * (A * p);
+                        for (Int_t i = 0; i < N - 1; i++)
+                        {
+                                Rinv(N - 1, i) = b(i);
+                                Rinv(i, N - 1) = b(i);
+                        }
+                        //
+                        Double_t pdotb = 0.;
+                        for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
+                        Double_t c = (1.0 - pdotb) / q;
+                        Rinv(N - 1, N - 1) = c;
+                }
+                else
+                {
+                        // case q = 0
+                        TMatrixDSym Qinv = RegInv(Q);           // Recursive call
+                        Double_t a = Qinv.Similarity(p);
+                        Double_t c = -1.0 / a;
+                        Rinv(N - 1, N - 1) = c;
+                        //
+                        TVectorD b = (1.0 / a) * (Qinv * p);
+                        for (Int_t i = 0; i < N - 1; i++)
+                        {
+                                Rinv(N - 1, i) = b(i);
+                                Rinv(i, N - 1) = b(i);
+                        }
+                        //
+                        A.Rank1Update(p, -1 / a);
+                        A += Q;
+                        A.Similarity(Qinv);
+                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
+                }
+        }
+        Minv = Rinv.Similarity(D);
+        return Minv;
+}
+//
+// Track tracjectory
+//
+TVector3 TrkUtil::Xtrack(TVectorD par, Double_t s)
+{
+        //
+        // unpack parameters
+        Double_t D = par(0);
+        Double_t p0 = par(1);
+        Double_t C = par(2);
+        Double_t z0 = par(3);
+        Double_t ct = par(4);
+        //
+        Double_t x = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
+        Double_t y =  D * TMath::Cos(p0) - (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);     
+        Double_t z = z0 + ct * s / (2 * C);
+        //
+        TVector3 Xt(x, y, z);
+        return Xt;
+}
+//
+// Track derivatives
+//
+// Constant radius
+// R-Phi
+TVectorD TrkUtil::derRphi_R(TVectorD par, Double_t R)
+{
+        TVectorD dRphi(5);      // return vector
+        //
+        // unpack parameters
+        Double_t D = par(0);
+        Double_t C = par(2);
+        //
+        Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D)));
+        TVector3 X = Xtrack(par, s);            // Intersection point
+        TVector3 v(-X.y()/R, X.x()/R, 0.);      // measurement direction
+        TMatrixD derX = derXdPar(par, s);       // dX/dp
+        TVectorD derXs = derXds(par, s);        // dX/ds
+        TVectorD ders = dsdPar_R(par, R);       // ds/dp        
+        //
+        for (Int_t i = 0; i < 5; i++)
+        {
+                dRphi(i) = 0.;
+                for (Int_t j = 0; j < 3; j++)
+                {
+                        dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+                }
+        }
+        //
+        return dRphi;
+}
+// z
+TVectorD TrkUtil::derZ_R(TVectorD par, Double_t R)
+{
+
+        TVectorD dZ(5); // return vector
+        //
+        // unpack parameters
+        Double_t D = par(0);
+        Double_t C = par(2);
+        //
+        Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D))); // phase
+        TVector3 v(0., 0., 1.);                         // measurement direction
+        TMatrixD derX = derXdPar(par, s);       // dX/dp
+        TVectorD derXs = derXds(par, s);        // dX/ds
+        TVectorD ders = dsdPar_R(par, R);       // ds/dp        
+        //
+        for (Int_t i = 0; i < 5; i++)
+        {
+                dZ(i) = 0.;
+                for (Int_t j = 0; j < 3; j++)
+                {
+                        dZ(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+                }
+        }
+        //
+        return dZ;
+}
+//
+// constant z
+// R-Phi
+TVectorD TrkUtil::derRphi_Z(TVectorD par, Double_t z)
+{
+        TVectorD dRphi(5);      // return vector
+        //
+        // unpack parameters
+        Double_t C = par(2);
+        Double_t z0 = par(3);
+        Double_t ct = par(4);
+        //
+        Double_t s = 2 * C * (z - z0) / ct;
+        TVector3 X = Xtrack(par, s);                    // Intersection point
+        TVector3 v(-X.y() / X.Pt(), X.x() / X.Pt(), 0.);        // measurement direction
+        TMatrixD derX = derXdPar(par, s);               // dX/dp
+        TVectorD derXs = derXds(par, s);                // dX/ds
+        TVectorD ders = dsdPar_z(par, z);               // ds/dp        
+        //
+        for (Int_t i = 0; i < 5; i++)
+        {
+                dRphi(i) = 0.;
+                for (Int_t j = 0; j < 3; j++)
+                {
+                        dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+                }
+        }
+        //
+        return dRphi;
+
+}
+// R
+TVectorD TrkUtil::derR_Z(TVectorD par, Double_t z)
+{
+        TVectorD dR(5); // return vector
+        //
+        // unpack parameters
+        Double_t C = par(2);
+        Double_t z0 = par(3);
+        Double_t ct = par(4);
+        //
+        Double_t s = 2 * C * (z - z0) / ct;
+        TVector3 X = Xtrack(par, s);                    // Intersection point
+        TVector3 v(X.x() / X.Pt(), X.y() / X.Pt(), 0.); // measurement direction
+        TMatrixD derX = derXdPar(par, s);               // dX/dp
+        TVectorD derXs = derXds(par, s);                // dX/ds
+        TVectorD ders = dsdPar_z(par, z);       // ds/dp        
+        //
+        for (Int_t i = 0; i < 5; i++)
+        {
+                dR(i) = 0.;
+                for (Int_t j = 0; j < 3; j++)
+                {
+                        dR(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
+                }
+        }
+        //
+        return dR;
+
+}
+//
+// derivatives of track trajectory
+//
+// dX/dPar
+TMatrixD TrkUtil::derXdPar(TVectorD par, Double_t s)
+{
+        TMatrixD dxdp(3, 5);    // return matrix
+        //
+        // unpack parameters
+        Double_t D = par(0);
+        Double_t p0 = par(1);
+        Double_t C = par(2);
+        Double_t z0 = par(3);
+        Double_t ct = par(4);
+        //
+        // derivatives
+        // dx/dD
+        dxdp(0, 0) = -TMath::Sin(p0);
+        dxdp(1, 0) =  TMath::Cos(p0);
+        dxdp(2, 0) = 0.;
+        // dx/dphi0
+        dxdp(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);
+        dxdp(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
+        dxdp(2, 1) = 0;
+        // dx/dC
+        dxdp(0, 2) = -(TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C * C);
+        dxdp(1, 2) =  (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C * C);
+        dxdp(2, 2) = -ct * s / (2 * C * C);
+        // dx/dz0
+        dxdp(0, 3) = 0;
+        dxdp(1, 3) = 0;
+        dxdp(2, 3) = 1.;
+        // dx/dCtg
+        dxdp(0, 4) = 0;
+        dxdp(1, 4) = 0;
+        dxdp(2, 4) = s / (2 * C);
+        //
+        return dxdp;
+}
+//
+// dX/ds
+//
+TVectorD TrkUtil::derXds(TVectorD par, Double_t s)
+{
+        TVectorD dxds(3);       // return vector
+        //
+        // unpack parameters
+        Double_t p0 = par(1);
+        Double_t C = par(2);
+        Double_t ct = par(4);
+        //
+        // dX/ds
+        dxds(0) = TMath::Cos(s + p0) / (2 * C);
+        dxds(1) = TMath::Sin(s + p0) / (2 * C);
+        dxds(2) = ct / (2 * C);
+        //
+        return dxds;
+}
+//
+// derivative of trajectory phase s
+//Constant R
+TVectorD TrkUtil::dsdPar_R(TVectorD par, Double_t R)
+{
+        TVectorD dsdp(5);       // return vector
+        //
+        // unpack parameters
+        Double_t D = par(0);
+        Double_t p0 = par(1);
+        Double_t C = par(2);
+        //
+        // derivatives
+        Double_t opCD = 1. + 2 * C * D;
+        Double_t A = C*TMath::Sqrt((R*R-D*D)/opCD);
+        Double_t sqA0 = TMath::Sqrt(1. - A * A);
+        Double_t dMin = 0.01;
+        Double_t sqA = TMath::Max(dMin, sqA0);  // Protect against divergence
+        //
+        dsdp(0) = -2 * C * C * (D * (1. + C * D) + C * R * R) / (A * sqA * opCD * opCD);
+        dsdp(1) = 0;
+        dsdp(2) = 2 * A * (1 + C * D) / (C * sqA * opCD);
+        dsdp(3) = 0;
+        dsdp(4) = 0;
+        //
+        return dsdp;
+}
+// Constant z
+TVectorD TrkUtil::dsdPar_z(TVectorD par, Double_t z)
+{
+        TVectorD dsdp(5);       // return vector
+        //
+        // unpack parameters
+        Double_t C = par(2);
+        Double_t z0 = par(3);
+        Double_t ct = par(4);
+        //
+        // derivatives
+        //
+        dsdp(0) = 0;
+        dsdp(1) = 0;
+        dsdp(2) = 2*(z-z0)/ct;
+        dsdp(3) = -2*C/ct;
+        dsdp(4) = -2*C*(z-z0)/(ct*ct);
+        //
+        return dsdp;
 }
 //