Changes in external/TrackCovariance/SolTrack.cc [ebf40fd:170a11d] in git
- File:
-
- 1 edited
-
external/TrackCovariance/SolTrack.cc (modified) (25 diffs)
Legend:
- Unmodified
- Added
- Removed
-
external/TrackCovariance/SolTrack.cc
rebf40fd r170a11d 1 2 #include "SolGeom.h" 3 #include "SolTrack.h" 1 #include <iostream> 2 4 3 #include <TString.h> 5 4 #include <TMath.h> … … 8 7 #include <TDecompChol.h> 9 8 #include <TMatrixDSymEigen.h> 10 #include <TGraph.h> 11 #include <iostream> 12 // 13 // Constructors 9 10 #include "SolGeom.h" 11 #include "SolTrack.h" 12 13 using namespace std; 14 14 15 SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G) 15 16 { 16 // Set B field 17 fG = G; // Store geometry 18 Double_t B = G->B(); 19 SetB(B); 20 // Store momentum and position 21 fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2]; 22 fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2]; 23 // Get generated parameters 24 TVector3 xv(fx); 25 TVector3 pv(fp); 26 Double_t Charge = 1.0; // Don't worry about charge for now 27 TVectorD gPar = XPtoPar(xv, pv, Charge); 28 // Store parameters 29 fpar[0] = gPar(0); 30 fpar[1] = gPar(1); 31 fpar[2] = gPar(2); 32 fpar[3] = gPar(3); 33 fpar[4] = gPar(4); 34 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl; 35 // 36 // Init covariances 37 // 38 fCov.ResizeTo(5, 5); 17 fG = G; 18 // Store momentum 19 fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2]; 20 Double_t px = p[0]; Double_t py = p[1]; Double_t pz = p[2]; 21 fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2]; 22 Double_t xx = x[0]; Double_t yy = x[1]; Double_t zz = x[2]; 23 // Store parameters 24 Double_t pt = TMath::Sqrt(px*px + py*py); 25 Double_t Charge = 1.0; // Don't worry about charge for now 26 Double_t a = -Charge*G->B()*0.2998; // Normalized speed of light 27 Double_t C = a / (2 * pt); // pt in GeV, B in Tesla, C in meters 28 Double_t r2 = xx*xx + yy*yy; 29 Double_t cross = xx*py - yy*px; 30 Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2); 31 Double_t phi0 = TMath::ATan2((py - a*xx) / T, (px + a*yy) / T); 32 Double_t D; 33 if (pt < 10.0) D = (T - pt) / a; 34 else D = (-2 * cross + a*r2) / (T + pt); 35 Double_t B = C*TMath::Sqrt((r2 - D*D) / (1 + 2 * C*D)); 36 Double_t st = TMath::ASin(B) / C; 37 Double_t ct = pz / pt; 38 Double_t z0 = zz - ct*st; 39 fpar[0] = D; 40 fpar[1] = phi0; 41 fpar[2] = C; 42 fpar[3] = z0; 43 fpar[4] = ct; 44 // Init covariances 45 fCov.ResizeTo(5, 5); 39 46 } 40 47 SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G) 41 48 { 42 // set B field 43 fG = G; // Store geometry 44 Double_t B = G->B(); 45 SetB(B); 49 fG = G; 46 50 // Store momentum 47 51 fp[0] = p(0); fp[1] = p(1); fp[2] = p(2); 52 Double_t px = p(0); Double_t py = p(1); Double_t pz = p(2); 48 53 fx[0] = x(0); fx[1] = x(1); fx[2] = x(2); 49 // Get generated parameters 54 Double_t xx = x(0); Double_t yy = x(1); Double_t zz = x(2); 55 // Store parameters 56 Double_t pt = TMath::Sqrt(px * px + py * py); 50 57 Double_t Charge = 1.0; // Don't worry about charge for now 51 TVectorD gPar = XPtoPar(x, p, Charge); 52 // Store parameters 53 fpar[0] = gPar(0); 54 fpar[1] = gPar(1); 55 fpar[2] = gPar(2); 56 fpar[3] = gPar(3); 57 fpar[4] = gPar(4); 58 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl; 59 // 60 // Init covariances 61 // 62 fCov.ResizeTo(5, 5); 63 } 64 // 65 SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G) 66 { 67 fG = G; 68 Double_t B = G->B(); 69 SetB(B); 70 // Store parameters 58 Double_t a = -Charge * G->B() * 0.2998; // Normalized speed of light 59 Double_t C = a / (2 * pt); // pt in GeV, B in Tesla, C in meters 60 Double_t r2 = xx * xx + yy * yy; 61 Double_t cross = xx * py - yy * px; 62 Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2); 63 Double_t phi0 = TMath::ATan2((py - a * xx) / T, (px + a * yy) / T); 64 Double_t D; 65 if (pt < 10.0) D = (T - pt) / a; 66 else D = (-2 * cross + a * r2) / (T + pt); 67 Double_t B = C * TMath::Sqrt((r2 - D * D) / (1 + 2 * C * D)); 68 Double_t st = TMath::ASin(B) / C; 69 Double_t ct = pz / pt; 70 Double_t z0 = zz - ct * st; 71 71 fpar[0] = D; 72 72 fpar[1] = phi0; … … 74 74 fpar[3] = z0; 75 75 fpar[4] = ct; 76 // Store momentum 77 Double_t pt = B * TrkUtil::cSpeed() / TMath::Abs(2 * C); 78 Double_t px = pt*TMath::Cos(phi0); 79 Double_t py = pt*TMath::Sin(phi0); 80 Double_t pz = pt*ct; 81 // 82 fp[0] = px; fp[1] = py; fp[2] = pz; 83 fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0); fx[2] = z0; 76 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl; 84 77 // 85 78 // Init covariances 86 79 // 87 80 fCov.ResizeTo(5, 5); 81 } 82 83 SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G) 84 { 85 fG = G; 86 // Store parameters 87 fpar[0] = D; 88 fpar[1] = phi0; 89 fpar[2] = C; 90 fpar[3] = z0; 91 fpar[4] = ct; 92 // Store momentum 93 Double_t pt = G->B()*0.2998 / TMath::Abs(2 * C); 94 Double_t px = pt*TMath::Cos(phi0); 95 Double_t py = pt*TMath::Sin(phi0); 96 Double_t pz = pt*ct; 97 98 fp[0] = px; fp[1] = py; fp[2] = pz; 99 fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0); fx[2] = z0; 100 // Init covariances 101 fCov.ResizeTo(5, 5); 88 102 } 89 103 // Destructor 90 104 SolTrack::~SolTrack() 91 105 { 92 fCov.Clear(); 93 } 94 // 106 delete[] & fp; 107 delete[] & fpar; 108 fCov.Clear(); 109 } 95 110 // Calculate intersection with given layer 96 111 Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz) 97 112 { 98 Double_t Di = D(); 99 Double_t phi0i = phi0(); 100 Double_t Ci = C(); 101 Double_t z0i = z0(); 102 Double_t cti = ct(); 103 // 104 R = 0; phi = 0; zz = 0; 105 Bool_t val = kFALSE; 106 Double_t Rmin = TMath::Sqrt(fx[0] * fx[0] + fx[1] * fx[1]); // Smallest track radius 107 if (Rmin < TMath::Abs(Di)) return val; 108 // 109 Double_t ArgzMin = Ci * TMath::Sqrt((Rmin * Rmin - Di * Di) / (1 + 2 * Ci * Di)); 110 Double_t stMin = TMath::ASin(ArgzMin) / Ci; // Arc length at track origin 111 // 112 if (fG->lTyp(il) == 1) // Cylinder: layer at constant R 113 { 114 R = fG->lPos(il); 115 Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di); 116 if (TMath::Abs(argph) < 1.0 && R > Rmin) 117 { 118 Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di)); 119 if (TMath::Abs(argz) < 1.0) 120 { 121 zz = z0i + cti*TMath::ASin(argz) / Ci; 122 if (zz > fG->lxMin(il) && zz < fG->lxMax(il)) 123 { 124 phi = phi0i + TMath::ASin(argph); 125 val = kTRUE; 126 } 127 } 128 } 129 } 130 else if (fG->lTyp(il) == 2) // disk: layer at constant z 131 { 132 zz = fG->lPos(il); 133 Double_t st = (zz - z0i) / cti; 134 if (TMath::Abs(Ci * st) < 1.0 && st > stMin) 135 { 136 R = TMath::Sqrt(Di * Di + (1. + 2. * Ci * Di) * pow(TMath::Sin(Ci * st), 2) / (Ci * Ci)); 137 if (R > fG->lxMin(il) && R < fG->lxMax(il)) 138 { 139 Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di); 140 if (TMath::Abs(arg1) < 1.0) 141 { 142 phi = phi0i + TMath::ASin(arg1); 143 val = kTRUE; 144 } 145 } 146 } 147 } 148 // 149 return val; 150 } 151 // 113 Double_t Di = D(); 114 Double_t phi0i = phi0(); 115 Double_t Ci = C(); 116 Double_t z0i = z0(); 117 Double_t cti = ct(); 118 119 R = 0; phi = 0; zz = 0; 120 121 Bool_t val = kFALSE; 122 if (fG->lTyp(il) == 1) // Cylinder: layer at constant R 123 { 124 R = fG->lPos(il); 125 Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di); 126 if (TMath::Abs(argph) < 1.0) 127 { 128 Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di)); 129 if (TMath::Abs(argz) < 1.0) 130 { 131 zz = z0i + cti*TMath::ASin(argz) / Ci; 132 if (zz > fG->lxMin(il) && zz < fG->lxMax(il)) 133 { 134 phi = phi0i + TMath::ASin(argph); 135 val = kTRUE; 136 } 137 } 138 } 139 } 140 else if (fG->lTyp(il) == 2) // disk: layer at constant z 141 { 142 zz = fG->lPos(il); 143 Double_t arg = Ci*(zz - z0i) / cti; 144 if (TMath::Abs(arg) < 1.0 && (zz - z0i) / cti > 0) 145 { 146 R = TMath::Sqrt(Di*Di + (1. + 2.*Ci*Di)*pow(TMath::Sin(arg), 2) / (Ci*Ci)); 147 if (R > fG->lxMin(il) && R < fG->lxMax(il)) 148 { 149 Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di); 150 if (TMath::Abs(arg1) < 1.0) 151 { 152 phi = phi0i + TMath::ASin(arg1); 153 val = kTRUE; 154 } 155 } 156 } 157 } 158 // 159 return val; 160 } 152 161 // # of layers hit 153 162 Int_t SolTrack::nHit() 154 163 { 155 156 157 158 159 // 160 161 } 162 // 164 Int_t kh = 0; 165 Double_t R; Double_t phi; Double_t zz; 166 for (Int_t i = 0; i < fG->Nl(); i++) 167 if (HitLayer(i, R, phi, zz))kh++; 168 169 return kh; 170 } 171 163 172 // # of measurement layers hit 164 173 Int_t SolTrack::nmHit() … … 172 181 } 173 182 // 183 184 174 185 // List of layers hit with intersections 175 186 Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh) 176 187 { 177 // 178 // Return lists of hits associated to a track including all scattering layers. 179 // Return value is the total number of measurement hits 180 // kmh = total number of measurement layers hit for given type 181 // ihh = pointer to layer number 182 // rhh = radius of hit 183 // zhh = z of hit 184 // 185 // ***** NB: double layers with stereo on lower layer not included 186 // 187 Int_t kh = 0; // Number of layers hit 188 Int_t kmh = 0; // Number of measurement layers of given type 189 for (Int_t i = 0; i < fG->Nl(); i++) 190 { 191 Double_t R; Double_t phi; Double_t zz; 192 if (HitLayer(i, R, phi, zz)) 193 { 194 zhh[kh] = zz; 195 rhh[kh] = R; 196 ihh[kh] = i; 197 if (fG->isMeasure(i))kmh++; 198 kh++; 199 } 200 } 201 // 202 return kmh; 203 } 204 // 188 // Return lists of hits associated to a track including all scattering layers. 189 // Return value is the total number of measurement hits 190 // kmh = total number of measurement layers hit for given type 191 // ihh = pointer to layer number 192 // rhh = radius of hit 193 // zhh = z of hit 194 195 // ***** NB: double layers with stereo on lower layer not included 196 197 Int_t kh = 0; // Number of layers hit 198 Int_t kmh = 0; // Number of measurement layers of given type 199 for (Int_t i = 0; i < fG->Nl(); i++) 200 { 201 Double_t R; Double_t phi; Double_t zz; 202 if (HitLayer(i, R, phi, zz)) // Only barrel type layers 203 { 204 zhh[kh] = zz; 205 rhh[kh] = R; 206 ihh[kh] = i; 207 if (fG->isMeasure(i))kmh++; 208 kh++; 209 } 210 } 211 212 return kmh; 213 } 214 205 215 // List of XYZ measurements without any error 206 216 Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh) 207 217 { 208 218 // 209 // Return lists of hits associated to a track for all measurement layers. 219 // Return lists of hits associated to a track for all measurement layers. 210 220 // Return value is the total number of measurement hits 211 221 // kmh = total number of measurement layers hit for given type … … 234 244 } 235 245 // 236 // Track plot237 //238 TGraph *SolTrack::TrkPlot()239 {240 //241 // Fill list of layers hit242 //243 Int_t Nhit = nHit(); // Total number of layers hit244 //cout << "Nhit = " << Nhit << endl;245 Double_t *zh = new Double_t[Nhit]; // z of hit246 Double_t *rh = new Double_t[Nhit]; // r of hit247 Int_t *ih = new Int_t [Nhit]; // true index of layer248 Int_t kmh; // Number of measurement layers hit249 //250 kmh = HitList(ih, rh, zh); // hit layer list251 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;252 Double_t *dh = new Double_t[Nhit]; // Hit distance from origin253 for(Int_t i=0; i<Nhit; i++)dh[i] = TMath::ASin(C() * TMath::Sqrt((rh[i] * rh[i] - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled;254 //255 Int_t *hord = new Int_t[Nhit];256 TMath::Sort(Nhit, dh, hord, kFALSE); // Order by increasing phase257 Double_t *z = new Double_t[Nhit]; // z of ordered hit258 Double_t *r = new Double_t[Nhit]; // r of ordered hit259 for (Int_t i = 0; i < Nhit; i++)260 {261 z[i] = zh[hord[i]];262 r[i] = rh[hord[i]];263 }264 //cout << "After ordering" << endl;265 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;266 TGraph *gr = new TGraph(Nhit, z, r); // graph intersection with layers267 gr->SetMarkerStyle(kCircle);268 gr->SetMarkerColor(kMagenta);269 gr->SetMarkerSize(1);270 gr->SetLineColor(kMagenta);271 //272 // clean up273 //274 delete[] zh;275 delete[] rh;276 delete[] ih;277 delete[] hord;278 return gr;279 }280 //281 246 // Covariance matrix estimation 282 247 // … … 285 250 // 286 251 // 287 // Input flags: 252 // Input flags: 288 253 // Res = .TRUE. turn on resolution effects/Use standard resolutions 289 254 // .FALSE. set all resolutions to 0 … … 300 265 Int_t ntry = 0; 301 266 Int_t ntrymax = 0; 302 Int_t Nhit = nHit(); // Total number of layers hit267 Int_t Nhit = nHit(); // Total number of layers hit 303 268 Double_t *zhh = new Double_t[Nhit]; // z of hit 304 269 Double_t *rhh = new Double_t[Nhit]; // r of hit 305 270 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin 306 271 Int_t *ihh = new Int_t[Nhit]; // true index of layer 307 Int_t kmh; // Number of measurement layers hit272 Int_t kmh; // Number of measurement layers hit 308 273 // 309 274 kmh = HitList(ihh, rhh, zhh); // hit layer list 310 Int_t mTot = 0; // Total number of measurements275 Int_t mTot = 0; // Total number of measurements 311 276 for (Int_t i = 0; i < Nhit; i++) 312 277 { 313 Double_t rr = rhh[i]; 314 dhh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled 278 dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]); 315 279 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements 316 280 } 317 281 // 318 // Order hit list by increasing arc length282 // Order hit list by increasing distance from origin 319 283 // 320 284 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin 321 TMath::Sort(Nhit, dhh, hord, kFALSE); 285 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin 322 286 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit 323 287 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit … … 333 297 // Store interdistances and multiple scattering angles 334 298 // 335 Double_t sn2t = 1.0 / (1 .0+ ct()*ct()); //sin^2 theta of track299 Double_t sn2t = 1.0 / (1 + ct()*ct()); //sin^2 theta of track 336 300 Double_t cs2t = 1.0 - sn2t; //cos^2 theta 337 301 Double_t snt = TMath::Sqrt(sn2t); // sin theta … … 342 306 // 343 307 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers 344 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane 345 Double_t* cs = new Double_t[Nhit]; // Cosine of angle with normal in transverse plane 346 // 308 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane 309 Double_t *cs = new Double_t[Nhit]; // Cosine of angle with layer normal 347 310 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop 348 311 { 349 312 Int_t i = ih[ii]; // Get true layer number 350 Int_t il = hord[ii]; // Unordered layer351 313 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D())); 352 //353 314 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer 354 315 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B); 355 316 Double_t pzi = pz0; 356 317 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D()); 357 //358 318 Double_t phi = phi0() + TMath::ASin(ArgRp); 359 319 Double_t nx = TMath::Cos(phi); // Barrel layer normal 360 320 Double_t ny = TMath::Sin(phi); 361 321 Double_t nz = 0.0; 362 cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt()); 363 // 364 if (fG->lTyp(i) == 2) // this is Z layer 322 if (fG->lTyp(i) == 2) // this is Z layer 365 323 { 366 324 nx = 0.0; … … 368 326 nz = 1.0; 369 327 } 370 Double_t corr = TMath::Abs(pxi*nx + pyi * ny + pzi * nz) / p(); 371 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction 328 Double_t corr = (pxi*nx + pyi * ny + pzi * nz) / p(); 329 cs[ii] = corr; 330 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction 372 331 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle 373 332 if (!MS)thms[ii] = 0; … … 375 334 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers 376 335 { 377 Int_t kl = hord[kk]; // Unordered layer 378 dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt; 336 //dik(ii, kk) = TMath::Sqrt(pow(rh[ii] - rh[kk], 2) + pow(zh[ii] - zh[kk], 2)); 337 Double_t Ci = C(); 338 dik(ii, kk) = (TMath::ASin(Ci*rh[ii])-TMath::ASin(Ci*rh[kk]))/(Ci*snt); 379 339 dik(kk, ii) = dik(ii, kk); 380 340 } 341 // 342 // Store momentum components for resolution correction cosines 343 // 344 Double_t *pRi = new Double_t[Nhit]; 345 pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component 346 Double_t *pPhi = new Double_t[Nhit]; 347 pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component 381 348 } 382 349 // 383 350 // Fill measurement covariance 384 351 // 385 TVectorD tPar(5,fpar); 386 // 352 Int_t *mTl = new Int_t[mTot]; // Pointer from measurement number to true layer number 387 353 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance 388 TMatrixD Rm(mTot, 5); // Derivative matrix389 Int_t im = 0; // Initialize number of measurement counter354 TMatrixD Rm(mTot, 5); // Derivative matrix 355 Int_t im = 0; 390 356 // 391 357 // Fill derivatives and error matrix with MS 392 358 // 359 Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.; 360 Double_t csMin = TMath::Cos(AngMx); // Set maximum angle wrt normal 361 // 393 362 for (Int_t ii = 0; ii < Nhit; ii++) 394 363 { 395 Int_t i = ih[ii]; // True layer number364 Int_t i = ih[ii]; // True layer number 396 365 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z 397 366 Int_t nmeai = fG->lND(i); // # measurements in layer 398 399 if (fG->isMeasure(i)) 367 if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin) 400 368 { 401 Double_t Ri = rh[ii]; 402 Double_t zi = zh[ii]; 369 Double_t Di = D(); // Get true track parameters 370 Double_t phi0i = phi0(); 371 Double_t Ci = C(); 372 Double_t z0i = z0(); 373 Double_t cti = ct(); 374 // 375 Double_t Ri = rh[ii]; 376 Double_t ArgRp = (Ri*Ci + (1 + Ci * Di)*Di / Ri) / (1 + 2 * Ci*Di); 377 Double_t ArgRz = Ci * TMath::Sqrt((Ri*Ri - Di * Di) / (1 + 2 * Ci*Di)); 378 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R 379 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R 380 // 381 // Derivative overflow protection 382 Double_t dMin = 0.8; 383 dRphi(0) = (1 - 2 * Ci*Ci*Ri*Ri) / 384 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // D derivative 385 dRphi(1) = Ri; // phi0 derivative 386 dRphi(2) = Ri * Ri / 387 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // C derivative 388 dRphi(3) = 0.0; // z0 derivative 389 dRphi(4) = 0.0; // cot(theta) derivative 390 // 391 dRz(0) = -cti * Di / 392 (Ri*TMath::Max(dMin,TMath::Sqrt(1 - Ci * Ci*Ri*Ri))); // D 393 dRz(1) = 0.0; // Phi0 394 dRz(2) = cti * (Ri*Ci / TMath::Sqrt(1-Ri*Ri*Ci*Ci) - // C 395 TMath::ASin(Ri*Ci)) / (Ci*Ci); 396 dRz(3) = 1.0; // Z0 397 dRz(4) = TMath::ASin(ArgRz) / Ci; // Cot(theta) 403 398 // 404 399 for (Int_t nmi = 0; nmi < nmeai; nmi++) 405 400 { 406 Double_t stri = 0; // Stereo angle 407 Double_t sig = 0; // Layer resolution 408 // Constant R derivatives 409 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R 410 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R 411 // 401 mTl[im] = i; 402 Double_t stri = 0; 403 Double_t sig = 0; 412 404 if (nmi + 1 == 1) // Upper layer measurements 413 405 { … … 415 407 Double_t csa = TMath::Cos(stri); 416 408 Double_t ssa = TMath::Sin(stri); 417 //418 409 sig = fG->lSgU(i); // Resolution 419 410 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R) 420 411 { 421 412 // 422 // Exact solution 423 dRphi = derRphi_R(tPar, Ri); 424 dRz = derZ_R (tPar, Ri); 425 // 413 // Almost exact solution (CD<<1, D<<R) 426 414 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 427 415 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 428 416 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 429 417 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 430 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 418 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 431 419 } 432 if (ityp == 2) // Z type layer (Measure R-phi at const. Z)420 if (ityp == 2) // Z type layer (Measure phi at const. Z) 433 421 { 434 TVectorD dRphz(5); dRphz.Zero(); // R-phi derivatives @ const. z 435 dRphz = derRphi_Z(tPar, zi); 436 // 437 Rm(im, 0) = dRphz(0); // D derivative 438 Rm(im, 1) = dRphz(1); // phi0 derivative 439 Rm(im, 2) = dRphz(2); // C derivative 440 Rm(im, 3) = dRphz(3); // z0 derivative 441 Rm(im, 4) = dRphz(4); // cot(theta) derivative 422 Rm(im, 0) = 1.0; // D derivative 423 Rm(im, 1) = rh[ii]; // phi0 derivative 424 Rm(im, 2) = rh[ii] * rh[ii]; // C derivative 425 Rm(im, 3) = 0; // z0 derivative 426 Rm(im, 4) = 0; // cot(theta) derivative 442 427 } 443 428 } … … 451 436 { 452 437 // 453 // Exact solution 454 dRphi = derRphi_R(tPar, Ri); 455 dRz = derZ_R (tPar, Ri); 456 // 438 // Almost exact solution (CD<<1, D<<R) 457 439 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 458 440 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative … … 463 445 if (ityp == 2) // Z type layer (Measure R at const. z) 464 446 { 465 TVectorD dRRz(5); dRRz.Zero(); // R derivatives @ const. z 466 dRRz = derR_Z(tPar, zi); 467 // 468 Rm(im, 0) = dRRz(0); // D derivative 469 Rm(im, 1) = dRRz(1); // phi0 derivative 470 Rm(im, 2) = dRRz(2); // C derivative 471 Rm(im, 3) = dRRz(3); // z0 derivative 472 Rm(im, 4) = dRRz(4); // cot(theta) derivative 447 Rm(im, 0) = 0; // D derivative 448 Rm(im, 1) = 0; // phi0 derivative 449 Rm(im, 2) = 0; // C derivative 450 Rm(im, 3) = -1.0 / ct(); // z0 derivative 451 Rm(im, 4) = -rh[ii] / ct(); // cot(theta) derivative 473 452 } 474 453 } … … 478 457 // 479 458 Int_t km = 0; 480 Double_t CosMin = TMath::Sin(TMath::Pi() / 9.); // Protect for derivative explosion481 459 for (Int_t kk = 0; kk <= ii; kk++) 482 460 { 483 Int_t k = ih[kk]; // True layer number484 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or disk461 Int_t k = ih[kk]; // True layer number 462 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or 485 463 Int_t nmeak = fG->lND(k); // # measurements in layer 486 if (fG->isMeasure(k) )464 if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin) 487 465 { 488 466 for (Int_t nmk = 0; nmk < nmeak; nmk++) 489 467 { 490 468 Double_t strk = 0; 491 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle upper 492 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle lower 493 //if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal 494 if (im == km && Res) { 495 Double_t sg = sig; 496 if(TMath::Abs(strk) < TMath::Pi()/6. && cs[kk] < CosMin) 497 TMath::Min(1000.*sig,sg = sig/pow(cs[kk],4)); 498 Sm(im, km) += sg * sg; // Detector resolution on diagonal 499 } 469 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle 470 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle 471 if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal 500 472 // 501 473 // Loop on all layers below for MS contributions 502 for (Int_t jj = 0; jj < kk; jj++) 474 for (Int_t jj = 0; jj < kk; jj++) 503 475 { 504 476 Double_t di = dik(ii, jj); … … 510 482 if (ktyp == 1) msk = ms / snt; // Barrel 511 483 else if (ktyp == 2) msk = ms / cst; // Disk 512 Double_t ci = TMath:: Abs(TMath::Cos(stri)); Double_t si = TMath::Abs(TMath::Sin(stri));513 Double_t ck = TMath:: Abs(TMath::Cos(strk)); Double_t sk = TMath::Abs(TMath::Sin(strk));514 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution484 Double_t ci = TMath::Cos(stri); Double_t si = TMath::Sin(stri); 485 Double_t ck = TMath::Cos(strk); Double_t sk = TMath::Sin(strk); 486 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution 515 487 } 516 488 // … … 525 497 } 526 498 Sm.ResizeTo(mTot, mTot); 527 TMatrixDSym SmTemp = Sm;528 499 Rm.ResizeTo(mTot, 5); 529 500 // … … 535 506 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id)); 536 507 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1 508 //SmN.Invert(); 537 509 // 538 510 // Protected matrix inversions 539 511 // 540 TDecompChol Chl(SmN ,1.e-12);512 TDecompChol Chl(SmN); 541 513 TMatrixDSym SmNinv = SmN; 542 514 if (Chl.Decompose()) … … 547 519 else 548 520 { 549 std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl; 550 //cout << "pt = " << pt() << endl; 521 cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl; 551 522 if (ntry < ntrymax) 552 523 { … … 554 525 ntry++; 555 526 } 556 //557 527 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN; 558 528 TDecompChol rChl(SmN); … … 563 533 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted 564 534 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian 535 // Normalize before inversion 565 536 const Int_t Npar = 5; 566 537 TMatrixDSym DHinv(Npar); DHinv.Zero(); … … 570 541 Hnrm.Invert(); 571 542 fCov = Hnrm.Similarity(DHinv); 572 // 573 // debug 574 // 575 if(TMath::IsNaN(fCov(0,0))) 576 { 577 std::cout<<"SolTrack::CovCalc: NaN found in covariance matrix"<<std::endl; 578 } 579 // 580 // Lots of cleanup to do 581 delete[] zhh; 582 delete[] rhh; 583 delete[] dhh; 584 delete[] ihh; 585 delete[] hord; 586 delete[] zh; 587 delete[] rh; 588 delete[] ih; 589 delete[] cs; 590 delete[] thms; 591 } 592 // 543 } 544 593 545 // Force positive definitness in normalized matrix 594 546 TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat) … … 606 558 if (Check) 607 559 { 608 std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl;560 cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << endl; 609 561 return rMatN; 610 562 } … … 614 566 TMatrixD U = Eign.GetEigenVectors(); 615 567 TVectorD lambda = Eign.GetEigenValues(); 616 //cout << "Eigenvalues:"; lambda.Print();617 //cout << "Input matrix: "; NormMat.Print();618 568 // Reset negative eigenvalues to small positive value 619 569 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13; … … 637 587 } 638 588 } 639 //cout << "Rebuilt matrix: "; rMatN.Print();640 589 return rMatN; 641 590 }
Note:
See TracChangeset
for help on using the changeset viewer.
