Changeset ebf40fd in git for external/TrackCovariance/SolTrack.cc
- Timestamp:
- Nov 29, 2021, 3:18:22 PM (3 years ago)
- Branches:
- master
- Children:
- bd376e3
- Parents:
- 9a7ea36
- File:
-
- 1 edited
-
external/TrackCovariance/SolTrack.cc (modified) (25 diffs)
Legend:
- Unmodified
- Added
- Removed
-
external/TrackCovariance/SolTrack.cc
r9a7ea36 rebf40fd 1 #include <iostream>2 1 2 #include "SolGeom.h" 3 #include "SolTrack.h" 3 4 #include <TString.h> 4 5 #include <TMath.h> … … 7 8 #include <TDecompChol.h> 8 9 #include <TMatrixDSymEigen.h> 9 10 #include "SolGeom.h" 11 #include "SolTrack.h" 12 13 using namespace std; 14 10 #include <TGraph.h> 11 #include <iostream> 12 // 13 // Constructors 15 14 SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G) 16 15 { 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); 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); 46 39 } 47 40 SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G) 48 41 { 49 fG = G; 42 // set B field 43 fG = G; // Store geometry 44 Double_t B = G->B(); 45 SetB(B); 50 46 // Store momentum 51 47 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);53 48 fx[0] = x(0); fx[1] = x(1); fx[2] = x(2); 54 Double_t xx = x(0); Double_t yy = x(1); Double_t zz = x(2); 49 // Get generated parameters 50 Double_t Charge = 1.0; // Don't worry about charge for now 51 TVectorD gPar = XPtoPar(x, p, Charge); 55 52 // Store parameters 56 Double_t pt = TMath::Sqrt(px * px + py * py); 57 Double_t Charge = 1.0; // Don't worry about charge for now 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; 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 71 71 fpar[0] = D; 72 72 fpar[1] = phi0; … … 74 74 fpar[3] = z0; 75 75 fpar[4] = ct; 76 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl; 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; 77 84 // 78 85 // Init covariances 79 86 // 80 87 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 parameters87 fpar[0] = D;88 fpar[1] = phi0;89 fpar[2] = C;90 fpar[3] = z0;91 fpar[4] = ct;92 // Store momentum93 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 covariances101 fCov.ResizeTo(5, 5);102 88 } 103 89 // Destructor 104 90 SolTrack::~SolTrack() 105 91 { 106 delete[] & fp; 107 delete[] & fpar; 108 fCov.Clear(); 109 } 92 fCov.Clear(); 93 } 94 // 110 95 // Calculate intersection with given layer 111 96 Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz) 112 97 { 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 } 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 // 161 152 // # of layers hit 162 153 Int_t SolTrack::nHit() 163 154 { 164 165 166 167 168 169 170 } 171 155 Int_t kh = 0; 156 Double_t R; Double_t phi; Double_t zz; 157 for (Int_t i = 0; i < fG->Nl(); i++) 158 if (HitLayer(i, R, phi, zz))kh++; 159 // 160 return kh; 161 } 162 // 172 163 // # of measurement layers hit 173 164 Int_t SolTrack::nmHit() … … 181 172 } 182 173 // 183 184 185 174 // List of layers hit with intersections 186 175 Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh) 187 176 { 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 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 // 215 205 // List of XYZ measurements without any error 216 206 Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh) 217 207 { 218 208 // 219 // Return lists of hits associated to a track for all measurement layers. 209 // Return lists of hits associated to a track for all measurement layers. 220 210 // Return value is the total number of measurement hits 221 211 // kmh = total number of measurement layers hit for given type … … 244 234 } 245 235 // 236 // Track plot 237 // 238 TGraph *SolTrack::TrkPlot() 239 { 240 // 241 // Fill list of layers hit 242 // 243 Int_t Nhit = nHit(); // Total number of layers hit 244 //cout << "Nhit = " << Nhit << endl; 245 Double_t *zh = new Double_t[Nhit]; // z of hit 246 Double_t *rh = new Double_t[Nhit]; // r of hit 247 Int_t *ih = new Int_t [Nhit]; // true index of layer 248 Int_t kmh; // Number of measurement layers hit 249 // 250 kmh = HitList(ih, rh, zh); // hit layer list 251 //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 origin 253 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 phase 257 Double_t *z = new Double_t[Nhit]; // z of ordered hit 258 Double_t *r = new Double_t[Nhit]; // r of ordered hit 259 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 layers 267 gr->SetMarkerStyle(kCircle); 268 gr->SetMarkerColor(kMagenta); 269 gr->SetMarkerSize(1); 270 gr->SetLineColor(kMagenta); 271 // 272 // clean up 273 // 274 delete[] zh; 275 delete[] rh; 276 delete[] ih; 277 delete[] hord; 278 return gr; 279 } 280 // 246 281 // Covariance matrix estimation 247 282 // … … 250 285 // 251 286 // 252 // Input flags: 287 // Input flags: 253 288 // Res = .TRUE. turn on resolution effects/Use standard resolutions 254 289 // .FALSE. set all resolutions to 0 … … 265 300 Int_t ntry = 0; 266 301 Int_t ntrymax = 0; 267 Int_t Nhit = nHit(); 302 Int_t Nhit = nHit(); // Total number of layers hit 268 303 Double_t *zhh = new Double_t[Nhit]; // z of hit 269 304 Double_t *rhh = new Double_t[Nhit]; // r of hit 270 305 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin 271 306 Int_t *ihh = new Int_t[Nhit]; // true index of layer 272 Int_t kmh; 307 Int_t kmh; // Number of measurement layers hit 273 308 // 274 309 kmh = HitList(ihh, rhh, zhh); // hit layer list 275 Int_t mTot = 0; 310 Int_t mTot = 0; // Total number of measurements 276 311 for (Int_t i = 0; i < Nhit; i++) 277 312 { 278 dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]); 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 279 315 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements 280 316 } 281 317 // 282 // Order hit list by increasing distance from origin318 // Order hit list by increasing arc length 283 319 // 284 320 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin 285 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin321 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin 286 322 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit 287 323 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit … … 297 333 // Store interdistances and multiple scattering angles 298 334 // 299 Double_t sn2t = 1.0 / (1 + ct()*ct()); //sin^2 theta of track335 Double_t sn2t = 1.0 / (1.0 + ct()*ct()); //sin^2 theta of track 300 336 Double_t cs2t = 1.0 - sn2t; //cos^2 theta 301 337 Double_t snt = TMath::Sqrt(sn2t); // sin theta … … 306 342 // 307 343 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers 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 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 // 310 347 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop 311 348 { 312 349 Int_t i = ih[ii]; // Get true layer number 350 Int_t il = hord[ii]; // Unordered layer 313 351 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D())); 352 // 314 353 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer 315 354 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B); 316 355 Double_t pzi = pz0; 317 356 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D()); 357 // 318 358 Double_t phi = phi0() + TMath::ASin(ArgRp); 319 359 Double_t nx = TMath::Cos(phi); // Barrel layer normal 320 360 Double_t ny = TMath::Sin(phi); 321 361 Double_t nz = 0.0; 322 if (fG->lTyp(i) == 2) // this is Z layer 362 cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt()); 363 // 364 if (fG->lTyp(i) == 2) // this is Z layer 323 365 { 324 366 nx = 0.0; … … 326 368 nz = 1.0; 327 369 } 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 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 331 372 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle 332 373 if (!MS)thms[ii] = 0; … … 334 375 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers 335 376 { 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); 377 Int_t kl = hord[kk]; // Unordered layer 378 dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt; 339 379 dik(kk, ii) = dik(ii, kk); 340 380 } 341 //342 // Store momentum components for resolution correction cosines343 //344 Double_t *pRi = new Double_t[Nhit];345 pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component346 Double_t *pPhi = new Double_t[Nhit];347 pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component348 381 } 349 382 // 350 383 // Fill measurement covariance 351 384 // 352 Int_t *mTl = new Int_t[mTot]; // Pointer from measurement number to true layer number 385 TVectorD tPar(5,fpar); 386 // 353 387 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance 354 TMatrixD Rm(mTot, 5); 355 Int_t im = 0; 388 TMatrixD Rm(mTot, 5); // Derivative matrix 389 Int_t im = 0; // Initialize number of measurement counter 356 390 // 357 391 // Fill derivatives and error matrix with MS 358 392 // 359 Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.;360 Double_t csMin = TMath::Cos(AngMx); // Set maximum angle wrt normal361 //362 393 for (Int_t ii = 0; ii < Nhit; ii++) 363 394 { 364 Int_t i = ih[ii]; 395 Int_t i = ih[ii]; // True layer number 365 396 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z 366 397 Int_t nmeai = fG->lND(i); // # measurements in layer 367 if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin) 368 { 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) 398 399 if (fG->isMeasure(i)) 400 { 401 Double_t Ri = rh[ii]; 402 Double_t zi = zh[ii]; 398 403 // 399 404 for (Int_t nmi = 0; nmi < nmeai; nmi++) 400 405 { 401 mTl[im] = i; 402 Double_t stri = 0; 403 Double_t sig = 0; 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 // 404 412 if (nmi + 1 == 1) // Upper layer measurements 405 413 { … … 407 415 Double_t csa = TMath::Cos(stri); 408 416 Double_t ssa = TMath::Sin(stri); 417 // 409 418 sig = fG->lSgU(i); // Resolution 410 419 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R) 411 420 { 412 421 // 413 // Almost exact solution (CD<<1, D<<R) 422 // Exact solution 423 dRphi = derRphi_R(tPar, Ri); 424 dRz = derZ_R (tPar, Ri); 425 // 414 426 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 415 427 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 416 428 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 417 429 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 418 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 430 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 419 431 } 420 if (ityp == 2) // Z type layer (Measure phi at const. Z)432 if (ityp == 2) // Z type layer (Measure R-phi at const. Z) 421 433 { 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 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 427 442 } 428 443 } … … 436 451 { 437 452 // 438 // Almost exact solution (CD<<1, D<<R) 453 // Exact solution 454 dRphi = derRphi_R(tPar, Ri); 455 dRz = derZ_R (tPar, Ri); 456 // 439 457 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 440 458 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative … … 445 463 if (ityp == 2) // Z type layer (Measure R at const. z) 446 464 { 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 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 452 473 } 453 474 } … … 457 478 // 458 479 Int_t km = 0; 480 Double_t CosMin = TMath::Sin(TMath::Pi() / 9.); // Protect for derivative explosion 459 481 for (Int_t kk = 0; kk <= ii; kk++) 460 482 { 461 Int_t k = ih[kk]; 462 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or 483 Int_t k = ih[kk]; // True layer number 484 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or disk 463 485 Int_t nmeak = fG->lND(k); // # measurements in layer 464 if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin)486 if (fG->isMeasure(k)) 465 487 { 466 488 for (Int_t nmk = 0; nmk < nmeak; nmk++) 467 489 { 468 490 Double_t strk = 0; 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 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 } 472 500 // 473 501 // Loop on all layers below for MS contributions 474 for (Int_t jj = 0; jj < kk; jj++) 502 for (Int_t jj = 0; jj < kk; jj++) 475 503 { 476 504 Double_t di = dik(ii, jj); … … 482 510 if (ktyp == 1) msk = ms / snt; // Barrel 483 511 else if (ktyp == 2) msk = ms / cst; // Disk 484 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); 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 contribution 487 515 } 488 516 // … … 497 525 } 498 526 Sm.ResizeTo(mTot, mTot); 527 TMatrixDSym SmTemp = Sm; 499 528 Rm.ResizeTo(mTot, 5); 500 529 // … … 506 535 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id)); 507 536 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1 508 //SmN.Invert();509 537 // 510 538 // Protected matrix inversions 511 539 // 512 TDecompChol Chl(SmN );540 TDecompChol Chl(SmN,1.e-12); 513 541 TMatrixDSym SmNinv = SmN; 514 542 if (Chl.Decompose()) … … 519 547 else 520 548 { 521 cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl; 549 std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl; 550 //cout << "pt = " << pt() << endl; 522 551 if (ntry < ntrymax) 523 552 { … … 525 554 ntry++; 526 555 } 556 // 527 557 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN; 528 558 TDecompChol rChl(SmN); … … 533 563 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted 534 564 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian 535 // Normalize before inversion536 565 const Int_t Npar = 5; 537 566 TMatrixDSym DHinv(Npar); DHinv.Zero(); … … 541 570 Hnrm.Invert(); 542 571 fCov = Hnrm.Similarity(DHinv); 543 } 544 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 // 545 593 // Force positive definitness in normalized matrix 546 594 TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat) … … 558 606 if (Check) 559 607 { 560 cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." <<endl;608 std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl; 561 609 return rMatN; 562 610 } … … 566 614 TMatrixD U = Eign.GetEigenVectors(); 567 615 TVectorD lambda = Eign.GetEigenValues(); 616 //cout << "Eigenvalues:"; lambda.Print(); 617 //cout << "Input matrix: "; NormMat.Print(); 568 618 // Reset negative eigenvalues to small positive value 569 619 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13; … … 587 637 } 588 638 } 639 //cout << "Rebuilt matrix: "; rMatN.Print(); 589 640 return rMatN; 590 641 }
Note:
See TracChangeset
for help on using the changeset viewer.
