Changeset dd263e4 in git for external/TrackCovariance
- Timestamp:
- Jan 19, 2022, 12:08:01 PM (3 years ago)
- Branches:
- master
- Children:
- 2c8865f, 56fb0be, 5c03893
- Parents:
- 0c0c9af (diff), 78ce8d1 (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@…> (01/19/22 12:08:01)
- git-committer:
- GitHub <noreply@…> (01/19/22 12:08:01)
- Location:
- external/TrackCovariance
- Files:
-
- 3 edited
Legend:
- Unmodified
- Added
- Removed
-
external/TrackCovariance/TrkUtil.h
r0c0c9af rdd263e4 68 68 // 69 69 static TVectorD XPtoPar(TVector3 x, TVector3 p, Double_t Q, Double_t Bz); 70 static TVector3 ParToX(TVectorD Par); 70 static TVector3 ParToX(TVectorD Par); // position of minimum distance from z axis 71 71 static TVector3 ParToP(TVectorD Par, Double_t Bz); // Get Momentum from track parameters 72 static Double_t ParToQ(TVectorD Par); 72 static Double_t ParToQ(TVectorD Par); // Get track charge 73 73 static void LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance); 74 74 // 75 75 // Track trajectory 76 76 // 77 static TVector3 Xtrack(TVectorD par, Double_t s); 78 TVectorD derRphi_R(TVectorD par, Double_t R); // Derivatives of R-phi at constant R77 static TVector3 Xtrack(TVectorD par, Double_t s); // Parametric track trajectory 78 TVectorD derRphi_R(TVectorD par, Double_t R); // Derivatives of R-phi at constant R 79 79 TVectorD derZ_R(TVectorD par, Double_t R); // Derivatives of z at constant R 80 TVectorD derRphi_Z(TVectorD par, Double_t z); // Derivatives of R-phi at constant z80 TVectorD derRphi_Z(TVectorD par, Double_t z); // Derivatives of R-phi at constant z 81 81 TVectorD derR_Z(TVectorD par, Double_t z); // Derivatives of R at constant z 82 82 // … … 88 88 // 89 89 static TVectorD ParToMm(TVectorD Par); // Parameter conversion 90 static TMatrixDSym CovToMm(TMatrixDSym Cov); // Covariance conversion90 static TMatrixDSym CovToMm(TMatrixDSym Cov); // Covariance conversion 91 91 // 92 92 // Inside cylindrical volume -
external/TrackCovariance/VertexFit.cc
r0c0c9af rdd263e4 1 #include <TMath.h> 1 /* 2 Vertex fitting code 3 */ 4 #include <TMath.h> 2 5 #include <TVectorD.h> 3 6 #include <TVector3.h> 4 7 #include <TMatrixD.h> 5 8 #include <TMatrixDSym.h> 9 #include <TMatrixDSymEigen.h> 6 10 #include "VertexFit.h" 7 11 // … … 38 42 for (Int_t i = 0; i < fNtr; i++) 39 43 { 40 TVectorD pr = *trkPar[i]; 41 fPar.push_back(new TVectorD(pr)); 42 fParNew.push_back(new TVectorD(pr)); 43 TMatrixDSym cv = *trkCov[i]; 44 fCov.push_back(new TMatrixDSym(cv)); 45 fCovNew.push_back(new TMatrixDSym(cv)); 44 fPar.push_back(new TVectorD(*trkPar[i])); 45 fParNew.push_back(new TVectorD(*trkPar[i])); 46 fCov.push_back(new TMatrixDSym(*trkCov[i])); 47 fCovNew.push_back(new TMatrixDSym(*trkCov[i])); 46 48 } 47 49 fChi2List.ResizeTo(fNtr); … … 76 78 void VertexFit::ResetWrkArrays() 77 79 { 78 Int_t N = (Int_t)ffi.size(); 79 for (Int_t i = 0; i < N; i++) 80 { 81 if (fx0i[i]) { fx0i[i]->Clear(); delete fx0i[i]; } 82 if (fai[i]) { fai[i]->Clear(); delete fai[i]; } 83 if (fdi[i]) { fdi[i]->Clear(); delete fdi[i]; } 84 if (fAti[i]) { fAti[i]->Clear(); delete fAti[i]; } 85 if (fDi[i]) { fDi[i]->Clear(); delete fDi[i]; } 86 if (fWi[i]) { fWi[i]->Clear(); delete fWi[i]; } 87 if (fWinvi[i]){ fWinvi[i]->Clear(); delete fWinvi[i]; } 88 } 89 fa2i.clear(); 90 fx0i.clear(); 91 fai.clear(); 92 fdi.clear(); 93 fAti.clear(); 94 fDi.clear(); 95 fWi.clear(); 96 fWinvi.clear(); 80 Int_t N = (Int_t)fdi.size(); 81 if(N > 0){ 82 for (Int_t i = 0; i < N; i++) 83 { 84 if (fx0i[i]) { fx0i[i]->Clear(); delete fx0i[i]; } 85 if (fai[i]) { fai[i]->Clear(); delete fai[i]; } 86 if (fdi[i]) { fdi[i]->Clear(); delete fdi[i]; } 87 if (fAti[i]) { fAti[i]->Clear(); delete fAti[i]; } 88 if (fDi[i]) { fDi[i]->Clear(); delete fDi[i]; } 89 if (fWi[i]) { fWi[i]->Clear(); delete fWi[i]; } 90 if (fWinvi[i]){ fWinvi[i]->Clear(); delete fWinvi[i]; } 91 } 92 fa2i.clear(); 93 fx0i.clear(); 94 fai.clear(); 95 fdi.clear(); 96 fAti.clear(); 97 fDi.clear(); 98 fWi.clear(); 99 fWinvi.clear(); 100 } 97 101 } 98 102 VertexFit::~VertexFit() 99 { 103 { 100 104 fxCst.Clear(); 101 105 fCovCst.Clear(); … … 103 107 fXv.Clear(); 104 108 fcovXv.Clear(); 105 fChi2List.Clear(); 109 fChi2List.Clear(); 106 110 // 107 111 for (Int_t i = 0; i < fNtr; i++) … … 111 115 fCov[i]->Clear(); delete fCov[i]; 112 116 fCovNew[i]->Clear(); delete fCovNew[i]; 113 } 117 } 114 118 fPar.clear(); 115 119 fParNew.clear(); … … 118 122 // 119 123 ResetWrkArrays(); 120 ffi.clear(); 124 ffi.clear(); 121 125 fNtr = 0; 122 126 } 123 127 // 124 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2) 125 { 126 // 127 // Find radius of minimum distance between two tracks 128 // 129 // p = (D,phi, C, z0, ct) 130 // 131 // Define arrays 132 // 133 Double_t C1 = p1(2); 134 Double_t C2 = p2(2); 135 Double_t ph1 = p1(1); 136 Double_t ph2 = p2(1); 137 TVectorD x0 = Fill_x0(p1); 138 TVectorD y0 = Fill_x0(p2); 139 TVectorD n = Fill_a(p1, 0.0); 140 n *= (2*C1); 141 TVectorD k = Fill_a(p2, 0.0); 142 k *= (2*C2); 143 // 144 // Setup and solve linear system 145 // 146 Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i); 147 Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i); 148 Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i); 149 Double_t discr = nn * kk - nk * nk; 150 TMatrixDSym H(2); 151 H(0, 0) = kk; 152 H(0, 1) = nk; 153 H(1, 0) = H(0, 1); 154 H(1, 1) = nn; 155 TVectorD c(2); 156 c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i)); 157 c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i)); 158 TVectorD smin = (H * c); 159 smin *= 1.0 / discr; 160 // 161 TVectorD X = x0 + smin(0) * n; 162 TVectorD Y = y0 + smin(1) * k; 163 // 164 // Higher order corrections 165 X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1); 166 X(1) += C1 * smin(0) * smin(0) * TMath::Cos(ph1); 167 Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2); 168 Y(1) += C2 * smin(1) * smin(1) * TMath::Cos(ph2); 169 // 170 TVectorD Xavg = 0.5 * (X + Y); 171 // 172 // 173 return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1)); 174 } 175 // 176 // Starting radius determination 177 Double_t VertexFit::StartRadius() 178 { 179 // 180 // Maximum impact parameter 181 Double_t Rd = 0; 182 for (Int_t i = 0; i < fNtr; i++) 183 { 184 TVectorD par = *fPar[i]; 185 Double_t Dabs = TMath::Abs(par(0)); 186 if (Dabs > Rd)Rd = Dabs; 187 } 188 //----------------------------- 189 // 190 // Find track pair with phi difference closest to pi/2 191 Int_t isel = 0; Int_t jsel = 0; // selected track indices 192 Double_t dSinMax = 0.0; // Max phi difference 193 for (Int_t i = 0; i < fNtr - 1; i++) 194 { 195 TVectorD pari = *fPar[i]; 196 Double_t phi1 = pari(1); 197 198 for (Int_t j = i + 1; j < fNtr; j++) 199 { 200 TVectorD parj = *fPar[j]; 201 Double_t phi2 = parj(1); 202 Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1)); 203 if (Sindphi > dSinMax) 204 { 205 isel = i; jsel = j; 206 dSinMax = Sindphi; 207 } 208 } 209 } 210 // 211 //------------------------------------------ 212 // 213 // Find radius of minimum distrance between tracks 214 TVectorD p1 = *fPar[isel]; 215 TVectorD p2 = *fPar[jsel]; 216 Double_t R = FastRv(p1, p2); 217 // 218 R = 0.9 * R + 0.1 * Rd; // Protect for overshoot 219 // 220 return R; 221 } 222 // 223 // Regularized symmetric matrix inversion 224 // 225 TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min) 226 { 227 TMatrixDSym M = Min; // Decouple from input 228 Int_t N = M.GetNrows(); // Matrix size 229 TMatrixDSym D(N); D.Zero(); // Normaliztion matrix 230 TMatrixDSym R(N); // Normarized matrix 231 TMatrixDSym Rinv(N); // Inverse of R 232 TMatrixDSym Minv(N); // Inverse of M 233 // 234 // Check for 0's and normalize 235 for (Int_t i = 0; i < N; i++) 236 { 237 if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i))); 238 else D(i, i) = 1.0; 239 } 240 R = M.Similarity(D); 241 // 242 // Recursive algorithms stops when N = 2 243 // 244 //**************** 245 // case N = 2 *** 246 //**************** 247 if (N == 2) 248 { 249 Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0); 250 if (det == 0) 251 { 252 std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl; 253 Rinv.Zero(); // Return null matrix 254 } 255 else 256 { 257 // invert matrix 258 Rinv(0, 0) = R(1, 1); 259 Rinv(0, 1) = -R(0, 1); 260 Rinv(1, 0) = Rinv(0, 1); 261 Rinv(1, 1) = R(0, 0); 262 Rinv *= 1. / det; 263 } 264 } 265 //**************** 266 // case N > 2 *** 267 //**************** 268 else 269 { 270 // Break up matrix 271 TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2); // Upper left 272 TVectorD p(N - 1); 273 for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i); 274 Double_t q = R(N - 1, N - 1); 275 //Invert pieces and re-assemble 276 TMatrixDSym Ainv(N - 1); 277 TMatrixDSym A(N - 1); 278 if (TMath::Abs(q) > 1.0e-15) 279 { 280 // Case |q| > 0 281 Ainv.Rank1Update(p, -1.0 / q); 282 Ainv += Q; 283 A = RegInv(Ainv); // Recursive call 284 TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A; 285 // 286 TVectorD b = (-1.0 / q) * (A * p); 287 for (Int_t i = 0; i < N - 1; i++) 288 { 289 Rinv(N - 1, i) = b(i); 290 Rinv(i, N - 1) = b(i); 291 } 292 // 293 Double_t pdotb = 0.; 294 for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i); 295 Double_t c = (1.0 - pdotb) / q; 296 Rinv(N - 1, N - 1) = c; 297 } 298 else 299 { 300 // case q = 0 301 TMatrixDSym Qinv = RegInv(Q); // Recursive call 302 Double_t a = Qinv.Similarity(p); 303 Double_t c = -1.0 / a; 304 Rinv(N - 1, N - 1) = c; 305 // 306 TVectorD b = (1.0 / a) * (Qinv * p); 307 for (Int_t i = 0; i < N - 1; i++) 308 { 309 Rinv(N - 1, i) = b(i); 310 Rinv(i, N - 1) = b(i); 311 } 312 // 313 A.Rank1Update(p, -1 / a); 314 A += Q; 315 A.Similarity(Qinv); 316 TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A; 317 } 318 } 319 Minv = Rinv.Similarity(D); 320 return Minv; 321 } 322 // 323 // 324 // 325 TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi) 326 { 327 // 328 // Derivative of track 3D position vector with respect to track parameters at constant phase 329 // 330 // par = vector of track parameters 331 // phi = phase 332 // 333 TMatrixD A(3, 5); 128 // 129 // 130 TVectorD VertexFit::Fill_x0(TVectorD par) 131 { 132 // 133 // Calculate track 3D position at R = |D| (minimum approach to z-axis) 134 // 135 TVectorD x0(3); 334 136 // 335 137 // Decode input arrays … … 341 143 Double_t ct = par(4); 342 144 // 343 // Fill derivative matrix dx/d alpha 344 // D 345 A(0, 0) = -TMath::Sin(p0); 346 A(1, 0) = TMath::Cos(p0); 347 A(2, 0) = 0.0; 348 // phi0 349 A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C); 350 A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C); 351 A(2, 1) = 0.0; 352 // C 353 A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C); 354 A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C); 355 A(2, 2) = -ct * phi / (2 * C * C); 356 // z0 357 A(0, 3) = 0.0; 358 A(1, 3) = 0.0; 359 A(2, 3) = 1.0; 360 // ct = lambda 361 A(0, 4) = 0.0; 362 A(1, 4) = 0.0; 363 A(2, 4) = phi / (2 * C); 364 // 365 return A; 366 } 367 // 368 TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi) 369 { 370 // 371 // Derivative of track 3D position vector with respect to phase at constant track parameters 372 // 373 // par = vector of track parameters 374 // phi = phase 375 // 376 TVectorD a(3); 145 x0(0) = -D * TMath::Sin(p0); 146 x0(1) = D * TMath::Cos(p0); 147 x0(2) = z0; 148 // 149 return x0; 150 } 151 // 152 TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi) 153 { 154 // 155 // Calculate track 3D position for a given phase, phi 156 // 157 TVectorD x(3); 377 158 // 378 159 // Decode input arrays … … 384 165 Double_t ct = par(4); 385 166 // 386 a(0) = TMath::Cos(phi + p0) / (2 * C);387 a(1) = TMath::Sin(phi + p0) / (2 * C);388 a(2) = ct / (2 * C);389 //390 return a;391 }392 //393 TVectorD VertexFit::Fill_x0(TVectorD par)394 {395 //396 // Calculate track 3D position at R = |D| (minimum approach to z-axis)397 //398 TVectorD x0(3);399 //400 // Decode input arrays401 //402 Double_t D = par(0);403 Double_t p0 = par(1);404 Double_t C = par(2);405 Double_t z0 = par(3);406 Double_t ct = par(4);407 //408 x0(0) = -D * TMath::Sin(p0);409 x0(1) = D * TMath::Cos(p0);410 x0(2) = z0;411 //412 return x0;413 }414 //415 TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)416 {417 //418 // Calculate track 3D position for a given phase, phi419 //420 TVectorD x(3);421 //422 // Decode input arrays423 //424 Double_t D = par(0);425 Double_t p0 = par(1);426 Double_t C = par(2);427 Double_t z0 = par(3);428 Double_t ct = par(4);429 //430 167 TVectorD x0 = Fill_x0(par); 431 168 x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C); … … 439 176 { 440 177 // 441 // Get track parameters and their covariance 178 // Get track parameters, covariance and phase 179 Double_t fs = ffi[i]; // Get phase 442 180 TVectorD par = *fParNew[i]; 443 181 TMatrixDSym Cov = *fCov[i]; 444 182 // 445 183 // Fill all track related work arrays arrays 446 Double_t fs = ffi[i]; // Get phase 184 TMatrixD A = derXdPar(par, fs); // A = dx/da = derivatives wrt track parameters 185 TMatrixDSym Winv = Cov; 186 Winv.Similarity(A); // W^-1 = A*C*A' 187 188 TMatrixD At(TMatrixD::kTransposed, A); // A transposed 189 fAti.push_back(new TMatrixD(At)); // Store A' 190 fWinvi.push_back(new TMatrixDSym(Winv)); // Store W^-1 matrix 191 // 447 192 TVectorD xs = Fill_x(par, fs); 448 193 fx0i.push_back(new TVectorD(xs)); // Start helix position 449 // 450 TMatrixD A = Fill_A(par, fs); // A = dx/da = derivatives wrt track parameters 451 TMatrixD At(TMatrixD::kTransposed, A); // A transposed 452 fAti.push_back(new TMatrixD(At)); // Store A' 453 TVectorD di = A * (par - *fPar[i]); // x-shift 454 fdi.push_back(new TVectorD(di)); // Store x-shift 455 TMatrixDSym Winv = Cov.Similarity(A); // W^-1 = A*C*A' 456 fWinvi.push_back(new TMatrixDSym(Winv)); // Store W^-1 matrix 194 // 195 TVectorD di = A * (par - *fPar[i]); // x-shift 196 fdi.push_back(new TVectorD(di)); // Store x-shift 457 197 // 458 198 TMatrixDSym W = RegInv(Winv); // W = (A*C*A')^-1 459 199 fWi.push_back(new TMatrixDSym(W)); // Store W matrix 460 200 // 461 TVectorD a = Fill_a(par, fs); // a = dx/ds = derivatives wrt phase201 TVectorD a = derXds(par, fs); // a = dx/ds = derivatives wrt phase 462 202 fai.push_back(new TVectorD(a)); // Store a 463 203 // … … 473 213 } 474 214 // 215 void VertexFit::VtxFitNoSteer() 216 { 217 // 218 // Initialize 219 // 220 std::vector<TVectorD*> x0i; // Tracks at ma 221 std::vector<TVectorD*> ni; // Track derivative wrt phase 222 std::vector<TMatrixDSym*> Ci; // Position error matrix at fixed phase 223 std::vector<TVectorD*> wi; // Ci*ni 224 // 225 // Track loop 226 for (Int_t i = 0; i < fNtr; i++) 227 { 228 Double_t s = 0.; 229 TVectorD par = *fPar[i]; 230 TMatrixDSym Cov = *fCov[i]; 231 x0i.push_back(new TVectorD(Fill_x0(par))); 232 ni.push_back(new TVectorD(derXds(par, s))); 233 TMatrixD A = derXdPar(par, s); 234 Ci.push_back(new TMatrixDSym(Cov.Similarity(A))); 235 TMatrixDSym Cinv = RegInv(*Ci[i]); 236 wi.push_back(new TVectorD(Cinv * (*ni[i]))); 237 } 238 //std::cout << "Vtx init completed. fNtr = "<<fNtr << std::endl; 239 // 240 // Get fit vertex 241 // 242 TMatrixDSym D(3); D.Zero(); 243 TVectorD Dx(3); Dx.Zero(); 244 for (Int_t i = 0; i < fNtr; i++) 245 { 246 TMatrixDSym Cinv = RegInv(*Ci[i]); 247 TMatrixDSym W(3); 248 W.Rank1Update(*wi[i], 1. / Ci[i]->Similarity(*wi[i])); 249 TMatrixDSym Dd = Cinv - W; 250 D += Dd; 251 Dx += Dd * (*x0i[i]); 252 } 253 if(fVtxCst){ 254 D += fCovCstInv; 255 Dx += fCovCstInv*fxCst; 256 } 257 fXv = RegInv(D) * Dx; 258 //std::cout << "Fast vertex (x, y, z) = "<<fXv(0)<<", "<<fXv(1)<<", "<<fXv(2) << std::endl; 259 // 260 // Get fit phases 261 // 262 for (Int_t i = 0; i < fNtr; i++){ 263 Double_t si = Dot(*wi[i], fXv - (*x0i[i])) / Ci[i]->Similarity(*wi[i]); 264 ffi.push_back(si); 265 //TVectorD xvi = Fill_x(*fPar[i],si); 266 //std::cout << "Fast vertex "<<i<<": xvi = "<<xvi(0)<<", "<<xvi(1)<<", "<<xvi(2) 267 // <<", si = "<<si<< std::endl; 268 } 269 // 270 // Cleanup 271 for (Int_t i = 0; i < fNtr; i++) 272 { 273 x0i[i]->Clear(); delete x0i[i]; 274 ni[i]->Clear(); delete ni[i]; 275 Ci[i]->Clear(); delete Ci[i]; 276 wi[i]->Clear(); delete wi[i]; 277 } 278 x0i.clear();; 279 ni.clear(); 280 Ci.clear(); 281 wi.clear(); 282 } 283 // 475 284 void VertexFit::VertexFitter() 476 285 { … … 487 296 TMatrixDSym covX(3); 488 297 Double_t Chi2 = 0; 489 TVectorD x0 = fXv; // If previous fit done 490 if (fRold < 0.0) { 491 // External constraint 492 if (fVtxCst) fRold = TMath::Sqrt(fxCst(0) * fxCst(0) + fxCst(1) * fxCst(1)); 493 // No constraint 494 else for (Int_t i = 0; i < 3; i++)x0(i) = 1000.; // Set to arbitrary large value 495 } 496 // 497 // Starting vertex radius approximation 498 // 499 Double_t R = fRold; // Use previous fit if available 500 if (R < 0.0) R = StartRadius(); // Rough vertex estimate 501 //std::cout << "Start radius: " << R << std::endl; 298 // 299 VtxFitNoSteer(); // Fast vertex finder on first pass (set ffi and fXv) 300 TVectorD x0 = fXv; 502 301 // 503 302 // Iteration properties … … 515 314 TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3); 516 315 covX.Zero(); // Reset vertex covariance 517 cterm.Zero(); // Reset constant term316 cterm.Zero(); // Reset constant term 518 317 H.Zero(); // Reset H matrix 519 318 DW1D.Zero(); … … 530 329 TVectorD par = *fPar[i]; 531 330 TMatrixDSym Cov = *fCov[i]; 532 //533 // For first iteration only534 Double_t fs;535 if (Ntry <= 0) // Initialize all phases on first pass536 {537 Double_t D = par(0);538 Double_t C = par(2);539 Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));540 fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));541 ffi.push_back(fs);542 }543 331 // 544 332 // Update track related arrays … … 555 343 // update constant term 556 344 TVectorD xs = *fx0i[i] - *fdi[i]; 557 TVectorD xx0 = *fx0i[i];558 /*559 std::cout << "Iter. " << Ntry << ", trk " << i << ", x= "560 << xx0(0) << ", " << xx0(1) << ", " << xx0(2)<<561 ", ph0= "<<par(1)<< std::endl;562 */345 //TVectorD xx0 = *fx0i[i]; 346 347 //std::cout << "Iter. " << Ntry << ", trk " << i << ", xs= " 348 // << xs(0) << ", " << xs(1) << ", " << xs(2)<< 349 // ", ph0= "<<par(1)<< std::endl; 350 563 351 cterm += Ds * xs; 564 352 } // End loop on tracks … … 584 372 TMatrixDSym Wm1 = *fWinvi[i]; 585 373 fChi2List(i) = Wm1.Similarity(lambda); 374 if(fChi2List(i) < 0.0){ 375 //std::cout<<"# "<<i<<", Chi2= "<<fChi2List(i)<<", Wm1:"<<std::endl; Wm1.Print(); 376 //std::cout<<"Lambda= "<<std::endl; lambda.Print(); 377 } 586 378 Chi2 += fChi2List(i); 587 379 TVectorD a = *fai[i]; 588 TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));589 f or (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];380 TVectorD b = (*fWi[i]) * (x - *fx0i[i] + *fdi[i]); 381 ffi[i] += Dot(a, b) / fa2i[i]; 590 382 TVectorD newPar = *fPar[i] - ((*fCov[i]) * (*fAti[i])) * lambda; 591 383 fParNew[i] = new TVectorD(newPar); … … 610 402 // 611 403 fVtxDone = kTRUE; // Set fit completion flag 612 fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1)); // Store fit radius 404 fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1)); // Store fit 405 //std::cout << "Found vertex " << fXv(0) << ", " << fXv(1) << ", " << fXv(2) 406 // << ", after "<<Ntry<<" iterations"<<std::endl; 613 407 // 614 408 } -
external/TrackCovariance/VertexFit.h
r0c0c9af rdd263e4 6 6 #include <TVectorD.h> 7 7 #include <TMatrixDSym.h> 8 #include "TrkUtil.h" 8 9 #include "ObsTrk.h" 9 10 #include <vector> … … 12 13 // Class for vertex fitting 13 14 14 class VertexFit { 15 class VertexFit: public TrkUtil 16 { 15 17 // 16 18 // Vertex fitting with track parameters steering … … 21 23 // 22 24 // Inputs 23 Int_t fNtr; // Number of tracks25 Int_t fNtr; // Number of tracks 24 26 std::vector<TVectorD*> fPar; // Input parameter array 25 27 std::vector<TVectorD*> fParNew; // Updated parameter array … … 27 29 std::vector<TMatrixDSym*> fCovNew; // Updated parameter covariances 28 30 // Constraints 29 Bool_t fVtxCst; // Vertex constraint flag30 TVectorD fxCst; // Constraint value31 Bool_t fVtxCst; // Vertex constraint flag 32 TVectorD fxCst; // Constraint value 31 33 TMatrixDSym fCovCst; // Constraint 32 34 TMatrixDSym fCovCstInv; // Inverse of constraint covariance 33 35 // 34 36 // Results 35 Bool_t fVtxDone; 37 Bool_t fVtxDone; // Flag vertex fit completed 36 38 Double_t fRold; // Current value of vertex radius 37 39 TVectorD fXv; // Found vertex … … 52 54 // 53 55 // Service routines 54 //void InitWrkArrays(); // Initializations55 56 void ResetWrkArrays(); // Clear work arrays 56 Double_t StartRadius(); // Starting vertex radius determination57 Double_t FastRv(TVectorD p1, TVectorD p2); // Fast vertex radius determination58 TMatrixDSym RegInv(TMatrixDSym& Smat0);// Regularized 3D matrix inversion59 TMatrixD Fill_A(TVectorD par, Double_t phi);// Derivative of track position wrt track parameters60 TVectorD Fill_a(TVectorD par, Double_t phi);// Derivative of track position wrt track phase57 //Double_t StartRadius(); // Starting vertex radius determination 58 //Double_t FastRv(TVectorD p1, TVectorD p2); // Fast vertex radius determination 59 //TMatrixDSym RegInv(TMatrixDSym& Smat0); // Regularized 3D matrix inversion 60 //TMatrixD Fill_A(TVectorD par, Double_t phi); // Derivative of track position wrt track parameters 61 //TVectorD Fill_a(TVectorD par, Double_t phi); // Derivative of track position wrt track phase 61 62 TVectorD Fill_x0(TVectorD par); // Track position at dma to z-axis 62 TVectorD Fill_x(TVectorD par, Double_t phi); 63 TVectorD Fill_x(TVectorD par, Double_t phi); // Track position at given phase 63 64 void UpdateTrkArrays(Int_t i); // Fill track realted arrays 64 void VertexFitter(); // Vertex finder routine 65 void VtxFitNoSteer(); // Vertex fitter routine w/o parameter steering 66 void VertexFitter(); // Vertex fitter routine w/ parameter steering 65 67 public: 66 68 //
Note:
See TracChangeset
for help on using the changeset viewer.