BOSS 6.6.4.p01
BESIII Offline Software System
Loading...
Searching...
No Matches
THelixFitter.cxx
Go to the documentation of this file.
1//-----------------------------------------------------------------------------
2// $Id: THelixFitter.cxx,v 1.35.8.1 2012/10/29 03:46:45 xuqn Exp $
3//-----------------------------------------------------------------------------
4// Filename : THelixFitter.cc
5// Section : Tracking
6// Owner : Yoshi Iwasaki
7// Email : [email protected]
8//-----------------------------------------------------------------------------
9// Description : A class to fit a TTrackBase object to a helix.
10// See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
11//-----------------------------------------------------------------------------
12
13/* for copysign */
14#if defined(__sparc)
15# if defined(__EXTENSIONS__)
16# include <cmath>
17# else
18# define __EXTENSIONS__
19# include <cmath>
20# undef __EXTENSIONS__
21# endif
22#elif defined(__GNUC__)
23# if defined(_XOPEN_SOURCE)
24# include <cmath>
25# else
26# define _XOPEN_SOURCE
27# include <cmath>
28# undef _XOPEN_SOURCE
29# endif
30#endif
31
32#define HEP_SHORT_NAMES
33#include <cfloat>
34//#include "panther/panther.h"
35//#include MDC_H
36#include "MdcTables/MdcTables.h"
37#include "CLHEP/Matrix/Vector.h"
38#include "CLHEP/Matrix/SymMatrix.h"
39#include "CLHEP/Matrix/DiagMatrix.h"
40#include "CLHEP/Matrix/Matrix.h"
41#include "TrkReco/THelixFitter.h"
42#include "TrkReco/TTrack.h"
43
44#include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
45#include "MdcCalibFunSvc/MdcCalibFunSvc.h"
46
47#include "GaudiKernel/ISvcLocator.h"
48#include "GaudiKernel/Bootstrap.h"
49
50#include "GaudiKernel/StatusCode.h"
51#include "GaudiKernel/IInterface.h"
52#include "GaudiKernel/Kernel.h"
53#include "GaudiKernel/Service.h"
54#include "GaudiKernel/ISvcLocator.h"
55#include "GaudiKernel/SvcFactory.h"
56#include "GaudiKernel/IDataProviderSvc.h"
57#include "GaudiKernel/Bootstrap.h"
58#include "GaudiKernel/MsgStream.h"
59#include "GaudiKernel/SmartDataPtr.h"
60#include "GaudiKernel/IMessageSvc.h"
61
62
63#include "EventModel/EventModel.h"
64#include "EvTimeEvent/RecEsTime.h"
65
66
67using CLHEP::HepVector;
68using CLHEP::HepSymMatrix;
69using CLHEP::HepDiagMatrix;
70using CLHEP::HepMatrix;
71
72// speed up option by j.tanaka (2001/04/14)
73#define OPTJT
74
75/*
76extern "C"
77void
78calcdc_driftdist_(int *,
79 int *,
80 int *,
81 float[3],
82 float[3],
83 float *,
84 float *,
85 float *);
86extern "C"
87void
88calcdc_driftdist3_(int *,
89 int *,
90 float[3],
91 float[3],
92 float *,
93 float[2],
94 float[2],
95 float[2]);
96
97extern "C"
98void
99calcdc_tof2_(int *, float *, float *, float *);
100*/
101
102#define NTrailMax 100
103#define Convergence 1.0e-5
104
105extern float
106TrkRecoHelixFitterChisqMax;
107
108#ifdef TRKRECO_DEBUG
109/*
110#include "tuple/BelleTupleManager.h"
111#include "TrkReco/TConformalFinder.h"
112#include "TrkReco/TMDCWireHitMC.h"
113BelleHistogram * _nCall[8];
114BelleHistogram * _nTrial[8];
115BelleHistogram * _pull[2][2][8];
116BelleHistogram * _nTrialNegative;
117BelleHistogram * _nTrialPositive;
118bool first = true;
119*/
120#endif
121
122THelixFitter::THelixFitter(const std::string & name)
123: TMFitter(name),
124 _fit2D(false),
125 _freeT0(false),
126 _sag(false),
127 _propagation(false),
128 _tof(false),
129 _tanl(false),
130 _pre_chi2(0.),
131 _fitted_chi2(0.),
132 m_pmgnIMF(0) {
133
134 //yzhang delete
135 //StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
136 //if(scmgn!=StatusCode::SUCCESS) {
137 // std::cout<< name <<" Unable to open Magnetic field service"<<std::endl;
138 //}else{
139 // std::cout<< name <<" open Magnetic field service"<<std::endl;
140 //}
141
142}
143
144THelixFitter::~THelixFitter() {
145}
146
147#ifdef OPTJT
148// speed up
149int
150THelixFitter::main(TTrackBase & b, float t0Offset,
151 double *pre_chi2, double *fitted_chi2) const {
152#ifdef TRKRECO_DEBUG
153/*
154 if (first) {
155 first = false;
156 extern BelleTupleManager * BASF_Histogram;
157 BelleTupleManager * m = BASF_Histogram;
158 _nCall[0] = m->histogram("HF nCall all", 1, 0., 1.);
159 _nCall[1] = m->histogram("HF nCall conf f2d l0", 1, 0., 1.);
160 _nCall[2] = m->histogram("HF nCall conf f3d l0", 1, 0., 1.);
161 _nCall[3] = m->histogram("HF nCall conf f2d l1", 1, 0., 1.);
162 _nCall[4] = m->histogram("HF nCall conf f3d l1", 1, 0., 1.);
163 _nCall[5] = m->histogram("HF nCall conf s2d", 1, 0., 1.);
164 _nCall[6] = m->histogram("HF nCall conf s3d", 1, 0., 1.);
165 _nCall[7] = m->histogram("HF nCall other", 1, 0., 1.);
166 _nTrial[0] = m->histogram("HF nTrial all", 100, 0., 100.);
167 _nTrial[1] = m->histogram("HF nTrial conf f2d l0", 100, 0., 100.);
168 _nTrial[2] = m->histogram("HF nTrial conf f3d l0", 100, 0., 100.);
169 _nTrial[3] = m->histogram("HF nTrial conf f2d l1", 100, 0., 100.);
170 _nTrial[4] = m->histogram("HF nTrial conf f3d l1", 100, 0., 100.);
171 _nTrial[5] = m->histogram("HF nTrial conf s2d", 100, 0., 100.);
172 _nTrial[6] = m->histogram("HF nTrial conf s3d", 100, 0., 100.);
173 _nTrial[7] = m->histogram("HF nTrial other", 100, 0., 100.);
174 _pull[0][0][0] = m->histogram("HF pull ax true all",
175 100, 0., 5000.);
176 _pull[0][0][2] = m->histogram("HF pull ax true conf f3d l0",
177 100, 0., 5000.);
178 _pull[0][0][4] = m->histogram("HF pull ax true conf f3d l1",
179 100, 0., 5000.);
180 _pull[0][0][6] = m->histogram("HF pull ax true conf s3d",
181 100, 0., 5000.);
182 _pull[0][0][7] = m->histogram("HF pull ax true other",
183 100, 0., 5000.);
184 _pull[1][0][0] = m->histogram("HF pull st true all",
185 100, 0., 5000.);
186 _pull[1][0][2] = m->histogram("HF pull st true conf f3d l0",
187 100, 0., 5000.);
188 _pull[1][0][4] = m->histogram("HF pull st true conf f3d l1",
189 100, 0., 5000.);
190 _pull[1][0][6] = m->histogram("HF pull st true conf s3d",
191 100, 0., 5000.);
192 _pull[1][0][7] = m->histogram("HF pull st true other",
193 100, 0., 5000.);
194 _pull[0][1][0] = m->histogram("HF pull ax wrong all",
195 100, 0., 5000.);
196 _pull[0][1][2] = m->histogram("HF pull ax wrong conf f3d l0",
197 100, 0., 5000.);
198 _pull[0][1][4] = m->histogram("HF pull ax wrong conf f3d l1",
199 100, 0., 5000.);
200 _pull[0][1][6] = m->histogram("HF pull ax wrong conf s3d",
201 100, 0., 5000.);
202 _pull[0][1][7] = m->histogram("HF pull ax wrong other",
203 100, 0., 5000.);
204 _pull[1][1][0] = m->histogram("HF pull st wrong all",
205 100, 0., 5000.);
206 _pull[1][1][2] = m->histogram("HF pull st wrong conf f3d l0",
207 100, 0., 5000.);
208 _pull[1][1][4] = m->histogram("HF pull st wrong conf f3d l1",
209 100, 0., 5000.);
210 _pull[1][1][6] = m->histogram("HF pull st wrong conf s3d",
211 100, 0., 5000.);
212 _pull[1][1][7] = m->histogram("HF pull st wrong other",
213 100, 0., 5000.);
214 _nTrialPositive = m->histogram("HF nTrial +", 100, 0., 100.);
215 _nTrialNegative = m->histogram("HF nTrial -", 100, 0., 100.);
216 }
217 _nCall[0]->accumulate(.5);
218 if (TConformalFinder::_stage == ConformalOutside)
219 _nCall[7]->accumulate(.5);
220 else if (TConformalFinder::_stage == ConformalFast2DLevel0)
221 _nCall[1]->accumulate(.5);
222 else if (TConformalFinder::_stage == ConformalFast3DLevel0)
223 _nCall[2]->accumulate(.5);
224 else if (TConformalFinder::_stage == ConformalFast2DLevel1)
225 _nCall[3]->accumulate(.5);
226 else if (TConformalFinder::_stage == ConformalFast3DLevel1)
227 _nCall[4]->accumulate(.5);
228 else if (TConformalFinder::_stage == ConformalSlow2D)
229 _nCall[5]->accumulate(.5);
230 else if (TConformalFinder::_stage == ConformalSlow3D)
231 _nCall[6]->accumulate(.5);
232 bool posi = true;
233 const TTrackHEP & hep = Links2HEP(b.links());
234*/
235#endif
236 //...Initialize
237 _pre_chi2 = _fitted_chi2 = 0.;
238 if(pre_chi2)*pre_chi2 = _pre_chi2;
239 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
240
241 //...Type check...
242 if (b.objectType() != Track) return TFitUnavailable;
243 TTrack & t = (TTrack &) b;
244
245 //...Already fitted ?...
246 if (t.fitted()) return TFitAlreadyFitted;
247
248 //...Count # of hits...
249 AList<TMLink> cores = t.cores();
250#ifdef TRKRECO_DEBUG
251 cout<<__FILE__<<" cores in helix = "<<cores.length()<<endl;
252#endif
253 if (_fit2D) cores = AxialHits(cores);
254 unsigned nCores = cores.length();
255 unsigned nStereoCores = NStereoHits(cores);
256
257 //...2D or 3D...
258 bool fitBy2D = _fit2D;
259 if (! fitBy2D) fitBy2D = (! nStereoCores);
260
261 //...Check # of hits...
262 if (! fitBy2D) {
263 if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
264 return TFitErrorFewHits;
265 }
266 else {
267 if (nCores < 3) return TFitErrorFewHits;
268 }
269
270 //...Setup...
271 Vector a(5), da(5);
272 a = t.helix().a();
273 Vector dxda(5);
274 Vector dyda(5);
275 Vector dzda(5);
276 Vector dDda(5);
277 Vector dchi2da(5);
278 SymMatrix d2chi2d2a(5, 0);
279 static const SymMatrix zero5(5, 0);
280 double chi2;
281 double chi2Old = DBL_MAX;
282 const double convergence = Convergence;
283 bool allAxial = true;
284 Matrix e(3, 3);
285 Vector f(3);
286 int err = 0;
287 double factor = 1.0;//jtanaka0715
288
289 //...For bad hit rejection...(by JT, 2001/04/12)...
290 int flagBad = 0;
291 if (TrkRecoHelixFitterChisqMax != 0.)
292 flagBad = 1;
293 AList<TMLink> initBadWires;
294 unsigned nInitBadWires = 0;
295 Vector initBadDchi2da(5);
296 SymMatrix initBadD2chi2d2a(5, 0);
297 for (unsigned j = 0; j < 5; ++j) initBadDchi2da[j] = 0.;
298 double initBadChi2 = 0.;
299
300 //...Fitting loop...
301 unsigned nTrial = 0;
302 while (nTrial < NTrailMax) {
303
304 //...Set up...
305 chi2 = 0.;
306 for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
307 d2chi2d2a = zero5;
308
309 //yuany
310#ifdef TRKRECO_DEBUG
311 cout<<"helix fitter a5 "<<t.helix().a()<< " cores.length "<<cores.length()<<endl;
312#endif
313 //...Loop with hits...
314 unsigned i = 0;
315 while (TMLink * l = cores[i++]) {
316 const TMDCWireHit & h = * l->hit();
317 //yuany
318#ifdef TRKRECO_DEBUG
319 cout<<"trial "<<nTrial<<" wire name "<<l->wire()->name()<<" L "<<(h.state()&WireHitPatternLeft)<<" R "<<(h.state()&WireHitPatternRight)<<" link "<<l->leftRight()<<endl;
320#endif
321 //...Cal. closest points...
322 t.approach(* l, _sag);
323 double dPhi = l->dPhi();
324 const HepPoint3D & onTrack = l->positionOnTrack();
325 const HepPoint3D & onWire = l->positionOnWire();
326 unsigned leftRight = (onWire.cross(onTrack).z() < 0.)
327 ? WireHitLeft : WireHitRight;
328
329 //...Obtain drift distance and its error...
330 double distance;
331 double eDistance;
332 drift(t, * l, t0Offset, distance, eDistance);
333 l->drift(distance,0);
334 l->drift(distance,1);
335 l->dDrift(eDistance,0);
336 l->dDrift(eDistance,1);
337
338
339 double inv_eDistance2 = 1. / (eDistance * eDistance);
340
341 //...Residual...
342 HepVector3D v = onTrack - onWire;
343 double vmag = v.mag();
344#ifdef TRKRECO_DEBUG
345 std::cout<<"THelixFitter::eDistance-----"<<eDistance<<endl;
346 cout<<" vmag = "<<vmag<<" distance = "<<distance<<" eDistance = "<<eDistance<<endl;
347#endif
348 double dDistance = vmag - distance;
349
350 //...dxda...
351 this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
352
353 //...Chi2 related...
354 // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
355 // Vector3 vw = h.wire()->direction();
356 double vw[3] = { h.wire()->direction().x(),
357 h.wire()->direction().y(),
358 h.wire()->direction().z() };
359 double vwxy = vw[0]*vw[1];
360 double vwyz = vw[1]*vw[2];
361 double vwzx = vw[2]*vw[0];
362 dDda = (vmag > 0.)
363 ? ((v.x() * (1. - vw[0] * vw[0]) -
364 v.y() * vwxy - v.z() * vwzx)
365 * dxda +
366 (v.y() * (1. - vw[1] * vw[1]) -
367 v.z() * vwyz - v.x() * vwxy)
368 * dyda +
369 (v.z() * (1. - vw[2] * vw[2]) -
370 v.x() * vwzx - v.y() * vwyz)
371 * dzda) / vmag
372 : Vector(5,0);
373 if (vmag <= 0.0) {
374 std::cout << " in fit " << onTrack << ", " << onWire;
375 h.dump();
376 }
377 double pChi2 = dDistance * dDistance * inv_eDistance2;
378#ifdef TRKRECO_DEBUG
379 std::cout<<"THelixFitter::dDistance"<<dDistance<<" inv_eDistance2 "<<inv_eDistance2<<endl;
380 cout<<"Liuqg check ... .. pChi2 = "<<pChi2<<endl;
381#endif
382
383 //...Bad hit rejection...
384 if (flagBad && nTrial == 0) {
385 if (pChi2 > TrkRecoHelixFitterChisqMax) {
386 initBadWires.append(l);
387 initBadDchi2da += (dDistance * inv_eDistance2) * dDda;
388 initBadD2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
389 initBadChi2 += pChi2;
390 }
391 }
392 else {
393 dchi2da += (dDistance * inv_eDistance2) * dDda;
394 d2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
395 chi2 += pChi2;
396
397 //...Store results...
398 l->update(onTrack, onWire, leftRight, pChi2);
399 }
400
401#ifdef TRKRECO_DEBUG
402// if ((! fitBy2D) && (nTrial == 0)) {
403// unsigned as = 0;
404// if (l->hit()->wire()->stereo()) as = 1;
405// unsigned mt = 0;
406// if (& hep != l->hit()->mc()->hep()) mt = 1;
407
408// _pull[as][mt][0]->accumulate(pChi2);
409// if (TConformalFinder::_stage == ConformalOutside)
410// _pull[as][mt][7]->accumulate(pChi2);
411// else if (TConformalFinder::_stage == ConformalFast3DLevel0)
412// _pull[as][mt][2]->accumulate(pChi2);
413// else if (TConformalFinder::_stage == ConformalFast3DLevel1)
414// _pull[as][mt][4]->accumulate(pChi2);
415// else if (TConformalFinder::_stage == ConformalSlow3D)
416// _pull[as][mt][6]->accumulate(pChi2);
417// }
418#endif
419 }
420
421 //...Bad hit rejection...
422 if (flagBad && nTrial == 0) {
423 if ((initBadWires.length() == 1 || initBadWires.length() == 2) &&
424 nCores >= 20 &&
425 chi2 / (double)(nCores - initBadWires.length()) < 10.) {
426 cores.remove(initBadWires);
427 nInitBadWires = initBadWires.length();
428 }
429 else if (initBadWires.length() != 0) {
430 dchi2da += initBadDchi2da;
431 d2chi2d2a += initBadD2chi2d2a;
432 chi2 += initBadChi2;
433 }
434 }
435
436 //...Save chi2 information...
437 if (nTrial == 0) {
438 _pre_chi2 = chi2;
439 _fitted_chi2 = chi2;
440 }
441 else {
442 _fitted_chi2 = chi2;
443 }
444
445 //...Check condition...
446 double change = chi2Old - chi2;
447#ifdef TRKRECO_DEBUG_DETAIL
448 std::cout<<" chi2 change "<<change <<" convergence "<<convergence <<" break? "<<(fabs(change) < convergence == true)<<std::endl;
449#endif
450 if (fabs(change) < convergence) break;
451 if (change < 0.) {
452// a += factor * da;
453// t._helix->a(a);
454// break;
455 factor = 0.5;
456 }
457 chi2Old = chi2;
458
459 //...Cal. helix parameters for next loop...
460 if (fitBy2D) {
461 f = dchi2da.sub(1, 3);
462 e = d2chi2d2a.sub(1, 3);
463 f = solve(e, f);
464 da[0] = f[0];
465 da[1] = f[1];
466 da[2] = f[2];
467 da[3] = 0.;
468 da[4] = 0.;
469 }
470 else {
471 da = solve(d2chi2d2a, dchi2da);
472 }
473
474 a -= factor * da;
475 t._helix->a(a);
476 ++nTrial;
477
478 // jtanaka 001008
479 //if( fabs(a[3]) > 200. ){
480 // yiwasaki 001010
481 if( fabs(a[3]) > 1000. ){
482 // stop "fit" and return error.
483 //std::cout << "Stop Fit... " << a << std::endl;
484 err = 1;
485 break;
486 }
487#ifdef TRKRECO_DEBUG_DETAIL
488 std::cout << " fit " << nTrial-1<< " : " << chi2 << " : "
489 << change << std::endl;
490#endif
491 }
492
493#ifdef TRKRECO_DEBUG
494/*
495 _nTrial[0]->accumulate(float(nTrial) + .5);
496 if (TConformalFinder::_stage == ConformalOutside)
497 _nTrial[7]->accumulate(float(nTrial) + .5);
498 else if (TConformalFinder::_stage == ConformalFast2DLevel0)
499 _nTrial[1]->accumulate(float(nTrial) + .5);
500 else if (TConformalFinder::_stage == ConformalFast3DLevel0)
501 _nTrial[2]->accumulate(float(nTrial) + .5);
502 else if (TConformalFinder::_stage == ConformalFast2DLevel1)
503 _nTrial[3]->accumulate(float(nTrial) + .5);
504 else if (TConformalFinder::_stage == ConformalFast3DLevel1)
505 _nTrial[4]->accumulate(float(nTrial) + .5);
506 else if (TConformalFinder::_stage == ConformalSlow2D)
507 _nTrial[5]->accumulate(float(nTrial) + .5);
508 else if (TConformalFinder::_stage == ConformalSlow3D)
509 _nTrial[6]->accumulate(float(nTrial) + .5);
510
511 if (posi) _nTrialPositive->accumulate((float) nTrial + .5);
512 else _nTrialNegative->accumulate((float) nTrial + .5);
513*/
514#endif
515
516 //...Cal. error matrix...
517 SymMatrix Ea(5, 0);
518 unsigned dim;
519 if (fitBy2D) {
520 dim = 3;
521 SymMatrix Eb(3, 0), Ec(3, 0);
522 Eb = d2chi2d2a.sub(1, 3);
523 Ec = Eb.inverse(err);
524 Ea[0][0] = Ec[0][0];
525 Ea[0][1] = Ec[0][1];
526 Ea[0][2] = Ec[0][2];
527 Ea[1][1] = Ec[1][1];
528 Ea[1][2] = Ec[1][2];
529 Ea[2][2] = Ec[2][2];
530 }
531 else {
532 dim = 5;
533 Ea = d2chi2d2a.inverse(err);
534 }
535
536 //...Store information...
537 if (! err) {
538 t._helix->a(a);
539 t._helix->Ea(Ea);
540 t._fitted = true;
541 }
542 else {
543 err = TFitFailed;
544 }
545
546 t._charge = copysign(1., a[2]);
547 t._ndf = nCores - dim -nInitBadWires;
548 t._chi2 = chi2;
549
550 //...Treatment for bad wires...
551 if (nInitBadWires) {
552 for (unsigned i = 0; i < nInitBadWires; i++) {
553 TMLink * l = initBadWires[i];
554 t.approach(* l, _sag);
555 double dPhi = l->dPhi();
556 const HepPoint3D & onTrack = l->positionOnTrack();
557 const HepPoint3D & onWire = l->positionOnWire();
558 HepVector3D v = onTrack - onWire;
559 double vmag = v.mag();
560 unsigned leftRight = (onWire.cross(onTrack).z() < 0.)
561 ? WireHitLeft : WireHitRight;
562 double distance;
563 double eDistance;
564 drift(t, * l, t0Offset, distance, eDistance);
565 l->drift(distance,0);
566 l->drift(distance,1);
567 l->dDrift(eDistance,0);
568 l->dDrift(eDistance,1);
569
570
571 double inv_eDistance2 = 1. / (eDistance * eDistance);
572 double dDistance = vmag - distance;
573 double pChi2 = dDistance * dDistance * inv_eDistance2;
574 l->update(onTrack, onWire, leftRight, pChi2);
575 }
576#ifdef TRKRECO_DEBUG_DETAIL
577 std::cout << " HelixFitter : # of rejected hits="
578 << nInitBadWires << endl;
579#endif
580 }
581
582 if (pre_chi2) * pre_chi2 = _pre_chi2;
583 if (fitted_chi2) * fitted_chi2 = _fitted_chi2;
584
585 return err;
586}
587#else
588int
589THelixFitter::main(TTrackBase & b, float t0Offset,
590 double *pre_chi2, double *fitted_chi2) const {
591#ifdef TRKRECO_DEBUG_DETAIL
592/*
593 if (first) {
594 first = false;
595 extern BelleTupleManager * BASF_Histogram;
596 BelleTupleManager * m = BASF_Histogram;
597 _nCall = m->histogram("HF nCall", 1, 0., 1.);
598 _nTrial = m->histogram("HF nTrial", 100, 0., 100.);
599 _nTrialPositive = m->histogram("HF nTrial +", 100, 0., 100.);
600 _nTrialNegative = m->histogram("HF nTrial -", 100, 0., 100.);
601 }
602 _nCall->accumulate(1.);
603 bool posi = true;
604 std::cout << " THelixFitter::fit ..." << std::endl;
605*/
606#endif
607 //...Initialize
608 _pre_chi2 = _fitted_chi2 = 0.;
609 if(pre_chi2)*pre_chi2 = _pre_chi2;
610 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
611
612 //...Type check...
613 if (b.objectType() != Track) return TFitUnavailable;
614 TTrack & t = (TTrack &) b;
615
616 //...Already fitted ?...
617 if (t.fitted()) return TFitAlreadyFitted;
618
619 //...Count # of hits...
620 AList<TMLink> cores = t.cores();
621 if (_fit2D) cores = AxialHits(cores);
622 unsigned nCores = cores.length();
623 unsigned nStereoCores = NStereoHits(cores);
624
625 //...2D or 3D...
626 bool fitBy2D = _fit2D;
627 if (! fitBy2D) fitBy2D = (! nStereoCores);
628
629 //...Check # of hits...
630 if (! fitBy2D) {
631 if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
632 return TFitErrorFewHits;
633 }
634 else {
635 if (nCores < 3) return TFitErrorFewHits;
636 }
637
638 //...Setup...
639 Vector a(5), da(5);
640 a = t.helix().a();
641 Vector dxda(5);
642 Vector dyda(5);
643 Vector dzda(5);
644 Vector dDda(5);
645 Vector dchi2da(5);
646 SymMatrix d2chi2d2a(5, 0);
647 static const SymMatrix zero5(5, 0);
648 double chi2;
649 double chi2Old = DBL_MAX;
650 const double convergence = Convergence;
651 bool allAxial = true;
652 Matrix e(3, 3);
653 Vector f(3);
654 int err = 0;
655 double factor = 1.0;//jtanaka0715
656
657 int flagBad = 0; // 2001/04/12
658 AList<TMLink> initBadWires;
659 unsigned nInitBadWires = 0;
660 Vector initBadDchi2da(5);
661 SymMatrix initBadD2chi2d2a(5, 0);
662 for (unsigned j = 0; j < 5; ++j) initBadDchi2da[j] = 0.;
663 double initBadChi2 = 0.;
664
665 //...Fitting loop...
666 unsigned nTrial = 0;
667 while (nTrial < NTrailMax) {
668
669 //...Set up...
670 chi2 = 0.;
671 for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
672 d2chi2d2a = zero5;
673
674 //...Loop with hits...
675 unsigned i = 0;
676 while (TMLink * l = cores[i++]) {
677 const TMDCWireHit & h = * l->hit();
678
679 //...Cal. closest points...
680 t.approach(* l, _sag);
681 double dPhi = l->dPhi();
682 const HepPoint3D & onTrack = l->positionOnTrack();
683 const HepPoint3D & onWire = l->positionOnWire();
684 unsigned leftRight = WireHitRight;
685 if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
686
687 //...Obtain drift distance and its error...
688 double distance;
689 double eDistance;
690 drift(t, * l, t0Offset, distance, eDistance);
691
692
693 double eDistance2 = eDistance * eDistance;
694
695 //...Residual...
696 HepVector3D v = onTrack - onWire;
697 double vmag = v.mag();
698 double dDistance = vmag - distance;
699
700 //...dxda...
701 this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
702
703 //...Chi2 related...
704 // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
705 HepVector3D vw = h.wire()->direction();
706 dDda = (vmag > 0.)
707 ? ((v.x() * (1. - vw.x() * vw.x()) -
708 v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
709 * dxda +
710 (v.y() * (1. - vw.y() * vw.y()) -
711 v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
712 * dyda +
713 (v.z() * (1. - vw.z() * vw.z()) -
714 v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
715 * dzda) / vmag
716 : Vector(5,0);
717 if (vmag <= 0.0) {
718 std::cout << " in fit " << onTrack << ", " << onWire;
719 h.dump();
720 }
721 double pChi2 = dDistance * dDistance / eDistance2;
722 if(flagBad){ // 2001/04/12
723 if(nTrial == 0 && pChi2 > 1500.){
724 initBadWires.append(l);
725 initBadDchi2da += (dDistance / eDistance2) * dDda;
726 initBadD2chi2d2a += vT_times_v(dDda) / eDistance2;
727 initBadChi2 += pChi2;
728 }else{
729 dchi2da += (dDistance / eDistance2) * dDda;
730 d2chi2d2a += vT_times_v(dDda) / eDistance2;
731 chi2 += pChi2;
732 //...Store results...
733 l->update(onTrack, onWire, leftRight, pChi2);
734 }
735 }else{
736 dchi2da += (dDistance / eDistance2) * dDda;
737 d2chi2d2a += vT_times_v(dDda) / eDistance2;
738 chi2 += pChi2;
739 //...Store results...
740 l->update(onTrack, onWire, leftRight, pChi2);
741 }
742 }
743
744 if(flagBad){ // 2001/04/12
745 if(nTrial == 0 &&
746 (initBadWires.length() == 1 ||
747 initBadWires.length() == 2) &&
748 nCores >= 20 &&
749 chi2/(double)(nCores-initBadWires.length()) < 10.){
750 cores.remove(initBadWires);
751 nInitBadWires = initBadWires.length();
752 }else if(nTrial == 0 && initBadWires.length() != 0){
753 dchi2da += initBadDchi2da;
754 d2chi2d2a += initBadD2chi2d2a;
755 chi2 += initBadChi2;
756 }
757 }
758
759 //...Save chi2 information...
760 if(nTrial == 0){
761 _pre_chi2 = chi2;
762 _fitted_chi2 = chi2;
763 }else _fitted_chi2 = chi2;
764
765 //...Check condition...
766 double change = chi2Old - chi2;
767 if (fabs(change) < convergence) break;
768 if (change < 0.) {
769// a += factor * da;
770// t._helix->a(a);
771// break;
772 factor = 0.5;
773 }
774 chi2Old = chi2;
775
776 //...Cal. helix parameters for next loop...
777 if (fitBy2D) {
778 f = dchi2da.sub(1, 3);
779 e = d2chi2d2a.sub(1, 3);
780 f = solve(e, f);
781 da[0] = f[0];
782 da[1] = f[1];
783 da[2] = f[2];
784 da[3] = 0.;
785 da[4] = 0.;
786 }
787 else {
788 da = solve(d2chi2d2a, dchi2da);
789 }
790
791 a -= factor * da;
792 t._helix->a(a);
793 ++nTrial;
794
795 // jtanaka 001008
796 //if( fabs(a[3]) > 200. ){
797 // yiwasaki 001010
798 if( fabs(a[3]) > 1000. ){
799 // stop "fit" and return error.
800 //std::cout << "Stop Fit... " << a << std::endl;
801 err = 1;
802 break;
803 }
804#ifdef TRKRECO_DEBUG_DETAIL
805 std::cout << " fit " << nTrial-1<< " : " << chi2 << " : "
806 << change << std::endl;
807#endif
808 }
809
810#ifdef TRKRECO_DEBUG_DETAIL
811/*
812 _nTrial->accumulate((float) nTrial + .5);
813 if (posi) _nTrialPositive->accumulate((float) nTrial + .5);
814 else _nTrialNegative->accumulate((float) nTrial + .5);
815*/
816#endif
817
818 //...Cal. error matrix...
819 SymMatrix Ea(5, 0);
820 unsigned dim;
821 if (fitBy2D) {
822 dim = 3;
823 SymMatrix Eb(3, 0), Ec(3, 0);
824 Eb = d2chi2d2a.sub(1, 3);
825 Ec = Eb.inverse(err);
826 Ea[0][0] = Ec[0][0];
827 Ea[0][1] = Ec[0][1];
828 Ea[0][2] = Ec[0][2];
829 Ea[1][1] = Ec[1][1];
830 Ea[1][2] = Ec[1][2];
831 Ea[2][2] = Ec[2][2];
832 }
833 else {
834 dim = 5;
835 Ea = d2chi2d2a.inverse(err);
836 }
837
838 //...Store information...
839 if (! err) {
840 t._helix->a(a);
841 t._helix->Ea(Ea);
842 t._fitted = true;
843 }
844 else {
845 err = TFitFailed;
846 }
847
848 t._charge = copysign(1., a[2]);
849 t._ndf = nCores - dim -nInitBadWires;
850 t._chi2 = chi2;
851
852 if(pre_chi2)*pre_chi2 = _pre_chi2;
853 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
854
855 return err;
856}
857#endif
858
859#ifdef OPTJT
860// speed up
861int
862THelixFitter::dxda(const TMLink & link,
863 const Helix & h,
864 double dPhi,
865 Vector & dxda,
866 Vector & dyda,
867 Vector & dzda) const {
868 // std::cout << "1st: " << m_pmgnIMF<< std::endl;
869 // maybe m_pmgnIMF == NULL here FIXME
870 if(!m_pmgnIMF) {
871 std::cout<<" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL "<<std::endl;
872 return 0;
873 }
874 //...Setup...
875 const TMDCWire & w = * link.wire();
876 const Vector & a = h.a();
877 const double dRho = a[0];
878 const double phi0 = a[1];
879 const double kappa = a[2];
880 // const double rho = Helix::ConstantAlpha / kappa;
881 // const double rho = 333.564095 / kappa;
882
883 const double Bz = -1000*m_pmgnIMF->getReferField();
884 const double rho = 333.564095/(kappa * Bz);
885 const double tanLambda = a[4];
886
887 const double sinPhi0 = sin(phi0);
888 const double cosPhi0 = cos(phi0);
889 // double sinPhi0 = h.sinphi0();
890 // double cosPhi0 = h.cosphi0();
891 const double sinPhi0dPhi = sin(phi0+dPhi);
892 const double cosPhi0dPhi = cos(phi0+dPhi);
893 //Vector dphida(5);
894 double dphida[5];
895
896 //...Sag correction...
897 HepPoint3D xw = w.xyPosition();
898 HepPoint3D wireBackwardPosition = w.backwardPosition();
899 HepVector3D v = w.direction();
900 if (_sag)
901 w.wirePosition(link.positionOnTrack().z(),
902 xw,
903 wireBackwardPosition,
904 v);
905 //yzhang 2012-08-30
906 //...Axial case...
907// if (w.axial()) {
908// const double d[3] = { h.center().x()-xw.x(),
909// h.center().y()-xw.y(),
910// h.center().z()-xw.z() };
911// const double inv_dmag2 = 1./(d[0]*d[0]+d[1]*d[1]+d[2]*d[2]);
912//
913// const double rho_per_kappa = rho/kappa;
914// const double dRho_plus_rho = dRho+rho;
915// const double rhoSinPhi0dPhi = rho*sinPhi0dPhi;
916// const double rhoCosPhi0dPhi = rho*cosPhi0dPhi;
917// const double rhoTanLambda = rho*tanLambda;
918//
919// dphida[0] = (sinPhi0*d[0]-cosPhi0*d[1])*inv_dmag2;
920// dphida[1] = dRho_plus_rho*(cosPhi0*d[0]+sinPhi0*d[1])*inv_dmag2-1.;
921// dphida[2] = -rho_per_kappa*dphida[0];
922// dphida[3] = 0.;
923// dphida[4] = 0.;
924//
925// dxda[0] = cosPhi0+rhoSinPhi0dPhi*dphida[0];
926// dxda[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi*(1.+dphida[1]);
927// dxda[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi)+rhoSinPhi0dPhi*dphida[2];
928// dxda[3] = 0.;
929// dxda[4] = 0.;
930//
931// dyda[0] = sinPhi0-rhoCosPhi0dPhi*dphida[0];
932// dyda[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi*(1.+dphida[1]);
933// dyda[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi)-rhoCosPhi0dPhi*dphida[2];
934// dyda[3] = 0.;
935// dyda[4] = 0.;
936//
937// dzda[0] = -rhoTanLambda*dphida[0];
938// dzda[1] = -rhoTanLambda*dphida[1];
939// dzda[2] = rho_per_kappa*tanLambda*dPhi-rhoTanLambda*dphida[2];
940// dzda[3] = 1.;
941// dzda[4] = -rho*dPhi;
942// }
943//
944// //...Stereo case...
945// else {
946 const double v_dot_wireBackwardPosition = v.x()*wireBackwardPosition.x()
947 + v.y()*wireBackwardPosition.y()
948 + v.z()*wireBackwardPosition.z();
949 const double c[3] = { w.backwardPosition().x()-v_dot_wireBackwardPosition*v.x(),
950 w.backwardPosition().y()-v_dot_wireBackwardPosition*v.y(),
951 w.backwardPosition().z()-v_dot_wireBackwardPosition*v.z() };
952
953 const double x[3] = { link.positionOnTrack().x(), link.positionOnTrack().y(), link.positionOnTrack().z() };
954 const double x_minus_c[3] = { x[0]-c[0], x[1]-c[1], x[2]-c[2] };
955
956 //Vector dxdphi(3);
957 const double dxdphi[3] = { rho*sinPhi0dPhi, -rho*cosPhi0dPhi, -rho*tanLambda };
958
959 //Vector d2xdphi2(3);
960 const double d2xdphi2[3] = { -dxdphi[1], dxdphi[0], 0. };
961
962 double dxdphi_dot_v = (dxdphi[0] * v.x() +
963 dxdphi[1] * v.y() +
964 dxdphi[2] * v.z());
965 double x_dot_v = x[0] * v.x() + x[1] * v.y() + x[2] * v.z();
966 double inv_dfdphi = -1./(( dxdphi[0] - dxdphi_dot_v*v.x()) * dxdphi[0]
967 +(dxdphi[1] - dxdphi_dot_v*v.y()) * dxdphi[1]
968 +(dxdphi[2] - dxdphi_dot_v*v.z()) * dxdphi[2]
969 +(x_minus_c[0] - x_dot_v*v.x()) * d2xdphi2[0]
970 +(x_minus_c[1] - x_dot_v*v.y()) * d2xdphi2[1]);
971 /* +(x_minus_c[2] - x_dot_v*v.z()) * d2xdphi2[2]; = 0. */
972
973
974 const double rho_per_kappa = rho/kappa;
975 const double dRho_plus_rho = dRho+rho;
976 const double &rhoSinPhi0dPhi = dxdphi[0];
977 const double rhoCosPhi0dPhi = -dxdphi[1];
978 const double rhoTanLambda = -dxdphi[2];
979
980 //dxda_phi, dyda_phi, dzda_phi : phi is fixed
981 //Vector dxda_phi(5);
982 double dxda_phi[5];
983 dxda_phi[0] = cosPhi0;
984 dxda_phi[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi;
985 dxda_phi[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi);
986 dxda_phi[3] = 0.;
987 dxda_phi[4] = 0.;
988
989 //Vector dyda_phi(5);
990 double dyda_phi[5];
991 dyda_phi[0] = sinPhi0;
992 dyda_phi[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi;
993 dyda_phi[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi);
994 dyda_phi[3] = 0.;
995 dyda_phi[4] = 0.;
996
997 //Vector dzda_phi(5);
998 double dzda_phi[5];
999 dzda_phi[0] = 0.;
1000 dzda_phi[1] = 0.;
1001 dzda_phi[2] = rho_per_kappa*tanLambda*dPhi;
1002 dzda_phi[3] = 1.;
1003 dzda_phi[4] = -rho*dPhi;
1004
1005 //Vector d2xdphida(5);
1006 double d2xdphida[5];
1007 d2xdphida[0] = 0.;
1008 d2xdphida[1] = rhoCosPhi0dPhi;
1009 d2xdphida[2] = -rho_per_kappa*sinPhi0dPhi;
1010 d2xdphida[3] = 0.;
1011 d2xdphida[4] = 0.;
1012
1013 //Vector d2ydphida(5);
1014 double d2ydphida[5];
1015 d2ydphida[0] = 0.;
1016 d2ydphida[1] = rhoSinPhi0dPhi;
1017 d2ydphida[2] = rho_per_kappa*cosPhi0dPhi;
1018 d2ydphida[3] = 0.;
1019 d2ydphida[4] = 0.;
1020
1021 //Vector d2zdphida(5);
1022 double d2zdphida[5];
1023 d2zdphida[0] = 0.;
1024 d2zdphida[1] = 0.;
1025 d2zdphida[2] = rho_per_kappa*tanLambda;
1026 d2zdphida[3] = 0.;
1027 d2zdphida[4] = -rho;
1028
1029 //Vector dfda(5);
1030 double dfda[5];
1031 for(int i = 0; i < 5; ++i) {
1032 double d_dot_v = (v.x() * dxda_phi[i] +
1033 v.y() * dyda_phi[i] +
1034 v.z() * dzda_phi[i]);
1035 dfda[i] = (- (dxda_phi[i] - d_dot_v * v.x()) * dxdphi[0]
1036 - (dyda_phi[i] - d_dot_v * v.y()) * dxdphi[1]
1037 - (dzda_phi[i] - d_dot_v * v.z()) * dxdphi[2]
1038 - (x_minus_c[0] - x_dot_v * v.x()) * d2xdphida[i]
1039 - (x_minus_c[1] - x_dot_v * v.y()) * d2ydphida[i]
1040 - (x_minus_c[2] - x_dot_v * v.z()) * d2zdphida[i]);
1041 dphida[i] = -dfda[i]*inv_dfdphi;
1042 }
1043
1044 dxda[0] = cosPhi0+rhoSinPhi0dPhi*dphida[0];
1045 dxda[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi*(1.+dphida[1]);
1046 dxda[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi)+rhoSinPhi0dPhi*dphida[2];
1047 dxda[3] = rhoSinPhi0dPhi*dphida[3];
1048 dxda[4] = rhoSinPhi0dPhi*dphida[4];
1049
1050 dyda[0] = sinPhi0-rhoCosPhi0dPhi*dphida[0];
1051 dyda[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi*(1.+dphida[1]);
1052 dyda[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi)-rhoCosPhi0dPhi*dphida[2];
1053 dyda[3] = -rhoCosPhi0dPhi*dphida[3];
1054 dyda[4] = -rhoCosPhi0dPhi*dphida[4];
1055
1056 dzda[0] = -rhoTanLambda*dphida[0];
1057 dzda[1] = -rhoTanLambda*dphida[1];
1058 dzda[2] = rho_per_kappa*tanLambda*dPhi-rhoTanLambda*dphida[2];
1059 dzda[3] = 1.-rhoTanLambda*dphida[3];
1060 dzda[4] = -rho*dPhi-rhoTanLambda*dphida[4];
1061 //}
1062
1063 //std::cout << dxda << std::endl;
1064 //std::cout << dyda << std::endl;
1065 //std::cout << dzda << std::endl;
1066
1067 return 0;
1068}
1069#else
1070int
1071THelixFitter::dxda(const TMLink & link,
1072 const Helix & h,
1073 double dPhi,
1074 Vector & dxda,
1075 Vector & dyda,
1076 Vector & dzda) const {
1077
1078 // std::cout << "2nd: " << m_pmgnIMF<< std::endl;
1079 //...Setup...
1080 const TMDCWire & w = * link.wire();
1081 const Vector & a = h.a();
1082 double dRho = a[0];
1083 double phi0 = a[1];
1084 double kappa = a[2];
1085 // double rho = Helix::ConstantAlpha / kappa;
1086 // double rho = 333.564095 / kappa;
1087
1088 // maybe m_pmgnIMF == NULL here FIXME
1089 if(!m_pmgnIMF) {
1090 std::cout<<" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL "<<std::endl;
1091 return 0;
1092 }
1093 double rho = 333.564095/(-1000*(m_pmgnIMF->getReferField())) / kappa;
1094
1095 double tanLambda = a[4];
1096
1097 double sinPhi0 = sin(phi0);
1098 double cosPhi0 = cos(phi0);
1099 // double sinPhi0 = h.sinphi0();
1100 // double cosPhi0 = h.cosphi0();
1101 double sinPhi0dPhi = sin(phi0 + dPhi);
1102 double cosPhi0dPhi = cos(phi0 + dPhi);
1103 Vector dphida(5);
1104
1105 //...Sag correction...
1106 HepPoint3D xw = w.xyPosition();
1107 HepPoint3D wireBackwardPosition = w.backwardPosition();
1108 HepVector3D v = w.direction();
1109 if (_sag)
1110 w.wirePosition(link.positionOnTrack().z(),
1111 xw,
1112 wireBackwardPosition,
1113 v);
1114 //yzhang 2012-08-30
1115 //...Axial case...
1116// if (w.axial()) {
1117// HepPoint3D d = h.center() - xw;
1118// double dmag2 = d.mag2();
1119//
1120// dphida[0] = (sinPhi0 * d.x() - cosPhi0 * d.y()) / dmag2;
1121// dphida[1] = (dRho + rho) * (cosPhi0 * d.x() + sinPhi0 * d.y())
1122// / dmag2 - 1.;
1123// dphida[2] = (- rho / kappa) * (sinPhi0 * d.x() - cosPhi0 * d.y())
1124// / dmag2;
1125// dphida[3] = 0.;
1126// dphida[4] = 0.;
1127// }
1128//
1129// //...Stereo case...
1130// else {
1131 //temp HepPoint3D onTrack = h.x(dPhi);
1132 //temp
1133 HepPoint3D onTrack = link.positionOnTrack();
1134 // std::cout << "ontrack =" << onTrack << std::endl;
1135 // std::cout << "ontrackp=" << onTrackp << std::endl;
1136 //temp
1137
1138 Vector c(3);
1139 c = HepPoint3D(w.backwardPosition() - (v * wireBackwardPosition) * v);
1140
1141 Vector x(3);
1142 x = onTrack;
1143
1144 Vector dxdphi(3);
1145 dxdphi[0] = rho * sinPhi0dPhi;
1146 dxdphi[1] = - rho * cosPhi0dPhi;
1147 dxdphi[2] = - rho * tanLambda;
1148
1149 Vector d2xdphi2(3);
1150 d2xdphi2[0] = rho * cosPhi0dPhi;
1151 d2xdphi2[1] = rho * sinPhi0dPhi;
1152 d2xdphi2[2] = 0.;
1153
1154 double dxdphi_dot_v = (dxdphi[0] * v.x() +
1155 dxdphi[1] * v.y() +
1156 dxdphi[2] * v.z());
1157 double x_dot_v = x[0] * v.x() + x[1] * v.y() + x[2] * v.z();
1158 double dfdphi = - (dxdphi[0] - dxdphi_dot_v*v.x()) * dxdphi[0]
1159 - (dxdphi[1] - dxdphi_dot_v*v.y()) * dxdphi[1]
1160 - (dxdphi[2] - dxdphi_dot_v*v.z()) * dxdphi[2]
1161 - (x[0] - c[0] - x_dot_v*v.x()) * d2xdphi2[0]
1162 - (x[1] - c[1] - x_dot_v*v.y()) * d2xdphi2[1];
1163 /* - (x[2] - c[2] - x_dot_v*v.z()) * d2xdphi2[2]; = 0. */
1164
1165
1166 //dxda_phi, dyda_phi, dzda_phi : phi is fixed
1167 Vector dxda_phi(5);
1168 dxda_phi[0] = cosPhi0;
1169 dxda_phi[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi;
1170 dxda_phi[2] = - (rho / kappa) * (cosPhi0 - cosPhi0dPhi);
1171 dxda_phi[3] = 0.;
1172 dxda_phi[4] = 0.;
1173
1174 Vector dyda_phi(5);
1175 dyda_phi[0] = sinPhi0;
1176 dyda_phi[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi;
1177 dyda_phi[2] = - (rho / kappa) * (sinPhi0 - sinPhi0dPhi);
1178 dyda_phi[3] = 0.;
1179 dyda_phi[4] = 0.;
1180
1181 Vector dzda_phi(5);
1182 dzda_phi[0] = 0.;
1183 dzda_phi[1] = 0.;
1184 dzda_phi[2] = (rho / kappa) * tanLambda * dPhi;
1185 dzda_phi[3] = 1.;
1186 dzda_phi[4] = - rho * dPhi;
1187
1188 Vector d2xdphida(5);
1189 d2xdphida[0] = 0.;
1190 d2xdphida[1] = rho * cosPhi0dPhi;
1191 d2xdphida[2] = - (rho / kappa) * sinPhi0dPhi;
1192 d2xdphida[3] = 0.;
1193 d2xdphida[4] = 0.;
1194
1195 Vector d2ydphida(5);
1196 d2ydphida[0] = 0.;
1197 d2ydphida[1] = rho * sinPhi0dPhi;
1198 d2ydphida[2] = (rho / kappa) * cosPhi0dPhi;
1199 d2ydphida[3] = 0.;
1200 d2ydphida[4] = 0.;
1201
1202 Vector d2zdphida(5);
1203 d2zdphida[0] = 0.;
1204 d2zdphida[1] = 0.;
1205 d2zdphida[2] = (rho / kappa) * tanLambda;
1206 d2zdphida[3] = 0.;
1207 d2zdphida[4] = - rho;
1208
1209 Vector dfda(5);
1210 for(int i = 0; i < 5; i++) {
1211 double d_dot_v = (v.x() * dxda_phi[i] +
1212 v.y() * dyda_phi[i] +
1213 v.z() * dzda_phi[i]);
1214 dfda[i] = (- (dxda_phi[i] - d_dot_v * v.x()) * dxdphi[0]
1215 - (dyda_phi[i] - d_dot_v * v.y()) * dxdphi[1]
1216 - (dzda_phi[i] - d_dot_v * v.z()) * dxdphi[2]
1217 - (x[0] - c[0] - x_dot_v * v.x()) * d2xdphida[i]
1218 - (x[1] - c[1] - x_dot_v * v.y()) * d2ydphida[i]
1219 - (x[2] - c[2] - x_dot_v * v.z()) * d2zdphida[i]);
1220 dphida[i] = - dfda[i] / dfdphi;
1221 }
1222 //}
1223
1224 dxda[0] = cosPhi0 + rho * sinPhi0dPhi * dphida[0];
1225 dxda[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi * (1. + dphida[1]);
1226 dxda[2] = - rho / kappa * (cosPhi0 - cosPhi0dPhi)
1227 + rho * sinPhi0dPhi * dphida[2];
1228 dxda[3] = rho * sinPhi0dPhi * dphida[3];
1229 dxda[4] = rho * sinPhi0dPhi * dphida[4];
1230
1231 dyda[0] = sinPhi0 - rho * cosPhi0dPhi * dphida[0];
1232 dyda[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi * (1. + dphida[1]);
1233 dyda[2] = - rho / kappa * (sinPhi0 - sinPhi0dPhi)
1234 - rho * cosPhi0dPhi * dphida[2];
1235 dyda[3] = - rho * cosPhi0dPhi * dphida[3];
1236 dyda[4] = - rho * cosPhi0dPhi * dphida[4];
1237
1238 dzda[0] = - rho * tanLambda * dphida[0];
1239 dzda[1] = - rho * tanLambda * dphida[1];
1240 dzda[2] = rho / kappa * tanLambda * dPhi - rho * tanLambda * dphida[2];
1241 dzda[3] = 1. - rho * tanLambda * dphida[3];
1242 dzda[4] = - rho * dPhi - rho * tanLambda * dphida[4];
1243
1244 //std::cout << dxda << std::endl;
1245 //std::cout << dyda << std::endl;
1246 //std::cout << dzda << std::endl;
1247
1248 return 0;
1249}
1250#endif
1251
1252void
1253THelixFitter::drift(const TTrack & t,
1254 TMLink & l,
1255 float t0Offset,
1256 double & distance,
1257 double & err) const {
1258 //be cautious here
1259 const TMDCWireHit & h = * l.hit();
1260 const HepPoint3D & onTrack = l.positionOnTrack();
1261 const HepPoint3D & onWire = l.positionOnWire();
1262 unsigned leftRight = WireHitRight;
1263 //yzhang delete TEMP
1264 if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
1265 int charge = (t.helix().a()[2]>0) ? 1: -1;//track charge//yzhang 2012-05-03 TEMP
1266 //m_pmgnIMF NULL set mutable 2012-05-04
1267 if(NULL == m_pmgnIMF) {
1268 Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
1269 if(NULL == m_pmgnIMF) {
1270 std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
1271 }
1272 }
1273 const double Bz = -1000*m_pmgnIMF->getReferField();
1274#ifdef TRKRECO_DEBUG
1275 std::cout << " orignal ambig "<<leftRight<<" charge "<< charge <<" Bz "<<Bz<<std::endl;
1276#endif
1277 //if(charge * Bz <0){
1278 // leftRight = (onWire.cross(onTrack).z() < 0.)
1279 // ? WireHitLeft : WireHitRight;
1280 //}else{
1281 // leftRight = (onWire.cross(onTrack).z() < 0.)
1282 // ? WireHitRight : WireHitLeft;
1283 //}
1284#ifdef TRKRECO_DEBUG
1285 std::cout << " new ambig "<<leftRight<<std::endl;
1286#endif
1287 //zhangy
1288
1289 //...No correction...
1290 if ((t0Offset == 0.) && (! _propagation) && (! _tof)) {
1291 distance = h.drift(leftRight);
1292 err = h.dDrift(leftRight);
1293 return;
1294 }
1295
1296// float x[3]={ onWire.x(), onWire.y(), onWire.z()};
1297// float Tcenter = 0;
1298
1299 //...TOF correction...
1300 float tof = 0.;
1301 if (_tof) {
1302 int imass = 3;
1303 float tl = t.helix().a()[4];
1304 float f = sqrt(1. + tl * tl);
1305 float s = fabs(t.helix().curv()) * fabs(l.dPhi()) * f;
1306 float p = f / fabs(t.helix().a()[2]);
1307//zsl calcdc_tof2_(& imass, & p, & s, & tof);
1308 float Mass[5] = {0.000511,0.105,0.140,0.493677,0.98327};
1309 tof = s/29.98*sqrt(1.0+(Mass[imass-1]/p)*(Mass[imass-1]/p));
1310
1311/* float x[3]={ onWire.x(), onWire.y(), onWire.z()};
1312 float Rad = 81; // cm
1313 float dRho = t.helix().a()[0];
1314 float Phi0 = t.helix().a()[1];
1315 float a2 = (dRho*cos(Phi0))*(dRho*cos(Phi0));
1316 float b2 = (dRho*sin(Phi0))*(dRho*sin(Phi0));
1317 float Lproj = sqrt(Rad*Rad - a2 - b2);
1318 Tcenter = Lproj/29.98; //approxiate... cm/ns
1319 tof = s/29.98; //cosmic
1320 if (x[1]>0) tof = -tof;
1321 */
1322 }
1323
1324 //...T0 and propagation corrections...
1325 int wire = h.wire()->localId();
1326 int layerId = h.wire()->layerId();
1327 int side = leftRight;
1328// if (side == 0) side = -1;
1329// HepVector3D tp = t.helix().momentum(l.dPhi());
1330// float p[3] = {tp.x(), tp.y(), tp.z()};
1331// float x[3] = {onWire.x(), onWire.y(), onWire.z()};
1332// float time = h.reccdc()->tdc + t0Offset - tof;
1333// float dist;
1334// float edist;
1335 double dist;
1336 double edist;
1337 int prop = _propagation;
1338
1339 const double x0 = t.helix().pivot().x();
1340 const double y0 = t.helix().pivot().y();
1341 Hep3Vector pivot_helix(x0,y0,0);
1342 const double dr = t.helix().a()[0];
1343 const double phi0 = t.helix().a()[1];
1344 const double kappa = t.helix().a()[2];
1345 const double dz = t.helix().a()[3];
1346 const double tanl = t.helix().a()[4];
1347
1348 //yzhang 2012-05-03 delete
1349 //const double alpha = 333.564095;
1350 //yzhang add
1351 const double alpha= 333.564095/(-1000*(m_pmgnIMF->getReferField()));
1352 //zhangy
1353
1354 const double cox = x0 + dr*cos(phi0) + alpha*cos(phi0)/kappa;
1355 const double coy = y0 + dr*sin(phi0) + alpha*sin(phi0)/kappa;
1356
1357
1358 unsigned nHits = t.links().length();
1359 unsigned nStereos = 0;
1360 unsigned firstLyr = 44;
1361 unsigned lastLyr = 0;
1362
1363
1364 HepPoint3D ontrack = l.positionOnTrack();
1365 HepPoint3D onwire = l.positionOnWire();
1366 HepPoint3D dir(ontrack.y()-coy,cox-ontrack.x(),0);
1367 double pos_phi=onwire.phi();
1368 double dir_phi=dir.phi();
1369 while(pos_phi>pi){pos_phi-=pi;}
1370 while(pos_phi<0){pos_phi+=pi;}
1371 while(dir_phi>pi){dir_phi-=pi;}
1372 while(dir_phi<0){dir_phi+=pi;}
1373 double entrangle=dir_phi-pos_phi;
1374 while(entrangle>pi/2)entrangle-=pi;
1375 while(entrangle<(-1)*pi/2)entrangle+=pi;
1376 /// by wangjk, propagation correction
1377 double zhit = onWire.z();
1378
1379#ifdef TRKRECO_DEBUG
1380 std::cout<<" onWire: "<<onWire<<std::endl;
1381 std::cout<<" zhit: "<<zhit<<std::endl;
1382#endif
1383
1384 const double vinner = 220.0e8; // unit is cm/s
1385 const double vouter = 240.0e8;
1386 double vprop = 300.0e9;
1387// double tprop = 0.;
1388
1389
1390 if(layerId<8) {
1391 vprop = vinner;
1392 }
1393 else {
1394 vprop = vouter;
1395 }
1396
1397
1398 double EsT0 = -1.;
1399 IMessageSvc *msgSvc;
1400 Gaudi::svcLocator()->service("MessageSvc", msgSvc);
1401 MsgStream log(msgSvc, "TCosmicFitter");
1402
1403 IDataProviderSvc* eventSvc = NULL;
1404 Gaudi::svcLocator()->service("EventDataSvc", eventSvc);
1405
1406 SmartDataPtr<RecEsTimeCol> aevtimeCol (eventSvc,"/Event/Recon/RecEsTimeCol");
1407 if (aevtimeCol) {
1408 RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
1409 EsT0 = (*iter_evt)->getTest();
1410 }else{
1411 log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
1412 }
1413
1414 double rawTime = 0.;
1415 rawTime = h.reccdc()->tdc;
1416 double rawadc = 0.;
1417 rawadc =h.reccdc()->adc;
1418 // double timewalk = CalibFunSvc_->getTimeWalk(layerid, 0.0);
1419 // double timeDrift = rawTime - tof - tprop - EsTo - toWalk;
1420
1421
1422 //----cal drift----//
1423 IMdcCalibFunSvc* l_mdcCalFunSvc;
1424 Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
1425
1426 double tprop = l_mdcCalFunSvc->getTprop(layerId,zhit*10.);
1427
1428 double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
1429
1430 double T0 = l_mdcCalFunSvc->getT0(layerId,wire);
1431 double drifttime = rawTime - tof - tprop - timeWalk -T0;
1432 l.setDriftTime(drifttime);
1433
1434#ifdef TRKRECO_DEBUG
1435 std::cout<<"timewalk is : "<< timeWalk << std::endl ;
1436 std::cout<<"T0 is : "<< T0 << std::endl ;
1437 std::cout<<"EsT0 is : "<< EsT0 << std::endl ;
1438 std::cout<<"tprop is : "<< tprop << std::endl ;
1439 std::cout<<"tof is : "<< tof << std::endl ;
1440 std::cout<<"rawTime is : "<< rawTime << std::endl ;
1441 std::cout<<"driftTime is : "<< drifttime << std::endl ;
1442#endif
1443// dist = l_mdcCalFunSvc->rawTimeNoTOFToDist(h.reccdc()->tdc - tof - tprop-timeWalk, layerId, wire, side, entrangle); //default entranceangle is 0
1444 dist = l_mdcCalFunSvc->driftTimeToDist(drifttime,layerId, wire, side, entrangle);//by liucy 2010/05/12
1445 edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, entrangle,tanl,zhit*10,rawadc);
1446 dist = dist/10.0; //mm->cm
1447 edist = edist/10.0;
1448
1449//zsl calcdc_driftdist_(& prop,
1450// & wire,
1451// & side,
1452// p,
1453// x,
1454// & time,
1455// & dist,
1456// & edist);
1457
1458// dist = (h.reccdc()->tdc-tof) * 40./10000;
1459
1460// distance = (double) dist;
1461// err = (double) edist;
1462 distance = dist;
1463 err = edist;
1464
1465 return;
1466}
1467
1468#ifdef OPTJT
1469//=====================================================================
1470int
1471THelixFitter::main(TTrackBase & b, float & tev, float & tev_err,
1472 double *pre_chi2, double *fitted_chi2) const {
1473//=====================================================================
1474 //...Initialize
1475 _pre_chi2 = _fitted_chi2 = 0.;
1476 if(pre_chi2)*pre_chi2 = _pre_chi2;
1477 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
1478
1479 //...Type check...
1480 if (b.objectType() != Track) return TFitUnavailable;
1481 TTrack & t = (TTrack &) b;
1482
1483 //...Already fitted ?...
1484 if (t.fitted()) return TFitAlreadyFitted;
1485
1486 //...Count # of hits...
1487 AList<TMLink> cores = t.cores();
1488 if (_fit2D) cores = AxialHits(cores);
1489 unsigned nCores = cores.length();
1490 unsigned nStereoCores = NStereoHits(cores);
1491
1492 //...2D or 3D...
1493 bool fitBy2D = _fit2D;
1494 if (! fitBy2D) fitBy2D = (! nStereoCores);
1495
1496 //...Check # of hits...
1497 if (! fitBy2D) {
1498 if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
1499 return TFitErrorFewHits;
1500 }
1501 else {
1502 if (nCores < 3) return TFitErrorFewHits;
1503 }
1504
1505 //...Setup...
1506 Vector a(6), da(6);
1507 Vector a_5dim(5);
1508 for (unsigned j = 0; j < 5; j++) a[j] = t.helix().a()[j];
1509 a[5] = tev;
1510 Vector dxda(5);
1511 Vector dyda(5);
1512 Vector dzda(5);
1513 Vector dDda(6);
1514 Vector dDda_5dim(5);
1515 Vector dchi2da(6);
1516 SymMatrix d2chi2d2a(6, 0);
1517 static const SymMatrix zero6(6, 0);
1518 double chi2;
1519 double chi2Old = DBL_MAX;
1520// const double convergence = Convergence;
1521 const double convergence = 1.0e-4; //Liuqg
1522 bool allAxial = true;
1523 Matrix e(3, 3);
1524 Vector f(3);
1525 int err = 0;
1526 double factor = 1.0;//jtanaka0715
1527
1528 //...Fitting loop...
1529 unsigned nTrial = 0;
1530 while (nTrial < NTrailMax) {
1531
1532 //...Set up...
1533 chi2 = 0.;
1534 for (unsigned j = 0; j < 6; j++) dchi2da[j] = 0.;
1535 d2chi2d2a = zero6;
1536
1537 //...Loop with hits...
1538 unsigned i = 0;
1539 while (TMLink * l = cores[i++]) {
1540 const TMDCWireHit & h = * l->hit();
1541
1542 //...Cal. closest points...
1543 t.approach(* l, _sag);
1544 double dPhi = l->dPhi();
1545 const HepPoint3D & onTrack = l->positionOnTrack();
1546 const HepPoint3D & onWire = l->positionOnWire();
1547 unsigned leftRight = (onWire.cross(onTrack).z() < 0.) ? WireHitLeft : WireHitRight;
1548
1549 //...Obtain drift distance and its error...
1550 double distance;
1551 double eDistance;
1552 double dddt;
1553 drift(t, * l, tev, distance, eDistance, dddt);
1554// l->drift(distance,0);
1555// l->drift(distance,1);
1556// l->dDrift(distance,0);
1557// l->dDrift(distance,1);
1558
1559
1560 double inv_eDistance2 = 1./(eDistance * eDistance);
1561
1562
1563 //...Residual...
1564 HepVector3D v = onTrack - onWire;
1565 double vmag = v.mag();
1566 double dDistance = vmag - distance;
1567
1568 //...dxda...
1569 this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
1570
1571 //...Chi2 related...
1572 // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
1573 double vw[3] = { h.wire()->direction().x(),
1574 h.wire()->direction().y(),
1575 h.wire()->direction().z() };
1576 double vwxy = vw[0]*vw[1];
1577 double vwyz = vw[1]*vw[2];
1578 double vwzx = vw[2]*vw[0];
1579 dDda_5dim = (vmag > 0.)
1580 ? ((v.x() * (1. - vw[0] * vw[0]) -
1581 v.y() * vwxy - v.z() * vwzx)
1582 * dxda +
1583 (v.y() * (1. - vw[1] * vw[1]) -
1584 v.z() * vwyz - v.x() * vwxy)
1585 * dyda +
1586 (v.z() * (1. - vw[2] * vw[2]) -
1587 v.x() * vwzx - v.y() * vwyz)
1588 * dzda) / vmag
1589 : Vector(5,0);
1590 if (vmag <= 0.0) {
1591 std::cout << " in fit " << onTrack << ", " << onWire;
1592 h.dump();
1593 }
1594 // for (unsigned j = 0; j < 5; j++) dDda[j] = dDda_5dim[j];
1595 dDda[0] = dDda_5dim[0];
1596 dDda[1] = dDda_5dim[1];
1597 dDda[2] = dDda_5dim[2];
1598 dDda[3] = dDda_5dim[3];
1599 dDda[4] = dDda_5dim[4];
1600 dDda[5] = -dddt;
1601
1602 dchi2da += (dDistance * inv_eDistance2) * dDda;
1603 d2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
1604 double pChi2 = dDistance * dDistance * inv_eDistance2;
1605 chi2 += pChi2;
1606
1607 //...Store results...
1608 l->update(onTrack, onWire, leftRight, pChi2);
1609 }
1610
1611 //...Save chi2 information...
1612 if(nTrial == 0){
1613 _pre_chi2 = chi2;
1614 _fitted_chi2 = chi2;
1615 }else _fitted_chi2 = chi2;
1616
1617 //...Check condition...
1618 double change = chi2Old - chi2;
1619 if (fabs(change) < convergence) break;
1620 //temp
1621 factor = 1.0;
1622 //temp
1623 if (change < 0.) {
1624// a += factor * da;
1625// t._helix->a(a);
1626// break;
1627 factor = 0.5;
1628 }
1629
1630 chi2Old = chi2;
1631
1632 //...Cal. helix parameters for next loop...
1633 if (fitBy2D) {
1634 f = dchi2da.sub(1, 4);
1635 e = d2chi2d2a.sub(1, 4);
1636 f = solve(e, f);
1637 da[0] = f[0];
1638 da[1] = f[1];
1639 da[2] = f[2];
1640 da[3] = f[3];
1641 da[4] = 0.;
1642 da[5] = 0.;
1643 }
1644 else {
1645 da = solve(d2chi2d2a, dchi2da);
1646 }
1647
1648 a -= factor * da;
1649
1650 // for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
1651 a_5dim[0] = a[0];
1652 a_5dim[1] = a[1];
1653 a_5dim[2] = a[2];
1654 a_5dim[3] = a[3];
1655 a_5dim[4] = a[4];
1656 t._helix->a(a_5dim);
1657 tev = a[5];
1658 //temp
1659 // if(nTrial == 0) std::cout << "initial chi2=" <<chi2 << std::endl;
1660 //temp
1661 ++nTrial;
1662
1663#ifdef TRKRECO_DEBUG
1664 std::cout << "fit " << nTrial-1<< " : " << chi2 << " : " << change << std::endl;
1665#endif
1666 }
1667
1668 //...Cal. error matrix...
1669 SymMatrix Ea(6, 0);
1670 unsigned dim;
1671 if (fitBy2D) {
1672 dim = 4;
1673 SymMatrix Eb(4, 0), Ec(4, 0);
1674 Eb = d2chi2d2a.sub(1, 4);
1675 Ec = Eb.inverse(err);
1676 Ea[0][0] = Ec[0][0];
1677 Ea[0][1] = Ec[0][1];
1678 Ea[0][2] = Ec[0][2];
1679 Ea[0][3] = Ec[0][3];
1680 Ea[1][1] = Ec[1][1];
1681 Ea[1][2] = Ec[1][2];
1682 Ea[1][3] = Ec[1][3];
1683 Ea[2][2] = Ec[2][2];
1684 Ea[2][3] = Ec[2][3];
1685 Ea[3][3] = Ec[3][3];
1686 }
1687 else {
1688 dim = 6;
1689 Ea = d2chi2d2a.inverse(err);
1690 // std::cout << "err flg=" << err << std::endl;
1691 }
1692
1693
1694 //...Store information...
1695 if (! err) {
1696 for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
1697 SymMatrix Ea_5dim(5, 0);
1698 Ea_5dim = Ea.sub(1, 5);
1699 t._helix->a(a_5dim);
1700 t._helix->Ea(Ea_5dim);
1701 tev = a[5];
1702 tev_err = sqrt(Ea[5][5]);
1703 //temp
1704 // std::cout << "nTrial=" << nTrial << std::endl;
1705 // std::cout << "chi2=" << chi2 << std::endl;
1706 // std::cout << "pt:" << fabs(1./a_5dim[2])<< std::endl;
1707 // std::cout << "tev,tev_err="<<tev<<" "<<tev_err<<std::endl;
1708 //temp
1709
1710 t._fitted = true;
1711 }
1712 else {
1713 err = TFitFailed;
1714 }
1715
1716 t._ndf = nCores - dim;
1717 t._chi2 = chi2;
1718
1719 if(pre_chi2)*pre_chi2 = _pre_chi2;
1720 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
1721
1722 return err;
1723}
1724#else
1725//=====================================================================
1726int
1727THelixFitter::main(TTrackBase & b, float & tev, float & tev_err,
1728 double *pre_chi2, double *fitted_chi2) const {
1729//=====================================================================
1730 //...Initialize
1731 _pre_chi2 = _fitted_chi2 = 0.;
1732 if(pre_chi2)*pre_chi2 = _pre_chi2;
1733 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
1734
1735 //...Type check...
1736 if (b.objectType() != Track) return TFitUnavailable;
1737 TTrack & t = (TTrack &) b;
1738
1739 //...Already fitted ?...
1740 if (t.fitted()) return TFitAlreadyFitted;
1741
1742 //...Count # of hits...
1743 AList<TMLink> cores = t.cores();
1744 if (_fit2D) cores = AxialHits(cores);
1745 unsigned nCores = cores.length();
1746 unsigned nStereoCores = NStereoHits(cores);
1747
1748 //...2D or 3D...
1749 bool fitBy2D = _fit2D;
1750 if (! fitBy2D) fitBy2D = (! nStereoCores);
1751
1752 //...Check # of hits...
1753 if (! fitBy2D) {
1754 if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
1755 return TFitErrorFewHits;
1756 }
1757 else {
1758 if (nCores < 3) return TFitErrorFewHits;
1759 }
1760
1761 //...Setup...
1762 Vector a(6), da(6);
1763 Vector a_5dim(5);
1764 for (unsigned j = 0; j < 5; j++) a[j] = t.helix().a()[j];
1765 a[5] = tev;
1766 Vector dxda(5);
1767 Vector dyda(5);
1768 Vector dzda(5);
1769 Vector dDda(6);
1770 Vector dDda_5dim(5);
1771 Vector dchi2da(6);
1772 SymMatrix d2chi2d2a(6, 0);
1773 static const SymMatrix zero6(6, 0);
1774 double chi2;
1775 double chi2Old = DBL_MAX;
1776 //temp const double convergence = Convergence;
1777 const double convergence = 1.0e-4;
1778 bool allAxial = true;
1779 Matrix e(3, 3);
1780 Vector f(3);
1781 int err = 0;
1782 double factor = 1.0;//jtanaka0715
1783
1784 //...Fitting loop...
1785 unsigned nTrial = 0;
1786 while (nTrial < NTrailMax) {
1787
1788 //...Set up...
1789 chi2 = 0.;
1790 for (unsigned j = 0; j < 6; j++) dchi2da[j] = 0.;
1791 d2chi2d2a = zero6;
1792
1793 //...Loop with hits...
1794 unsigned i = 0;
1795 while (TMLink * l = cores[i++]) {
1796 const TMDCWireHit & h = * l->hit();
1797
1798 //...Cal. closest points...
1799 t.approach(* l, _sag);
1800 double dPhi = l->dPhi();
1801 const HepPoint3D & onTrack = l->positionOnTrack();
1802 const HepPoint3D & onWire = l->positionOnWire();
1803 unsigned leftRight = WireHitRight;
1804 if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
1805
1806 //...Obtain drift distance and its error...
1807 double distance;
1808 double eDistance;
1809 double dddt;
1810 drift(t, * l, tev, distance, eDistance, dddt);
1811
1812
1813 double eDistance2 = eDistance * eDistance;
1814
1815 //...Residual...
1816 HepVector3D v = onTrack - onWire;
1817 double vmag = v.mag();
1818 double dDistance = vmag - distance;
1819
1820 //...dxda...
1821 this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
1822
1823 //...Chi2 related...
1824 // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
1825 HepVector3D vw = h.wire()->direction();
1826 dDda_5dim = (vmag > 0.)
1827 ? ((v.x() * (1. - vw.x() * vw.x()) -
1828 v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
1829 * dxda +
1830 (v.y() * (1. - vw.y() * vw.y()) -
1831 v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
1832 * dyda +
1833 (v.z() * (1. - vw.z() * vw.z()) -
1834 v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
1835 * dzda) / vmag
1836 : Vector(5,0);
1837 if (vmag <= 0.0) {
1838 std::cout << " in fit " << onTrack << ", " << onWire;
1839 h.dump();
1840 }
1841 // for (unsigned j = 0; j < 5; j++) dDda[j] = dDda_5dim[j];
1842 dDda[0] = dDda_5dim[0];
1843 dDda[1] = dDda_5dim[1];
1844 dDda[2] = dDda_5dim[2];
1845 dDda[3] = dDda_5dim[3];
1846 dDda[4] = dDda_5dim[4];
1847 dDda[5] = -dddt;
1848
1849 dchi2da += (dDistance / eDistance2) * dDda;
1850 d2chi2d2a += vT_times_v(dDda) / eDistance2;
1851 double pChi2 = dDistance * dDistance / eDistance2;
1852 chi2 += pChi2;
1853
1854 //...Store results...
1855 l->update(onTrack, onWire, leftRight, pChi2);
1856 }
1857
1858 //...Save chi2 information...
1859 if(nTrial == 0){
1860 _pre_chi2 = chi2;
1861 _fitted_chi2 = chi2;
1862 }else _fitted_chi2 = chi2;
1863
1864 //...Check condition...
1865 double change = chi2Old - chi2;
1866 if (fabs(change) < convergence) break;
1867 //temp
1868 factor = 1.0;
1869 //temp
1870 if (change < 0.) {
1871// a += factor * da;
1872// t._helix->a(a);
1873// break;
1874 factor = 0.5;
1875 }
1876 chi2Old = chi2;
1877
1878 //...Cal. helix parameters for next loop...
1879 if (fitBy2D) {
1880 f = dchi2da.sub(1, 4);
1881 e = d2chi2d2a.sub(1, 4);
1882 f = solve(e, f);
1883 da[0] = f[0];
1884 da[1] = f[1];
1885 da[2] = f[2];
1886 da[3] = f[3];
1887 da[4] = 0.;
1888 da[5] = 0.;
1889 }
1890 else {
1891 da = solve(d2chi2d2a, dchi2da);
1892 }
1893
1894 a -= factor * da;
1895
1896 // for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
1897 a_5dim[0] = a[0];
1898 a_5dim[1] = a[1];
1899 a_5dim[2] = a[2];
1900 a_5dim[3] = a[3];
1901 a_5dim[4] = a[4];
1902 t._helix->a(a_5dim);
1903 tev = a[5];
1904 //temp
1905 // if(nTrial == 0) std::cout << "initial chi2=" <<chi2 << std::endl;
1906 //temp
1907 ++nTrial;
1908
1909#ifdef TRKRECO_DEBUG
1910 std::cout << "fit " << nTrial-1<< " : " << chi2 << " : " << change << std::endl;
1911#endif
1912 }
1913
1914 //...Cal. error matrix...
1915 SymMatrix Ea(6, 0);
1916 unsigned dim;
1917 if (fitBy2D) {
1918 dim = 4;
1919 SymMatrix Eb(4, 0), Ec(4, 0);
1920 Eb = d2chi2d2a.sub(1, 4);
1921 Ec = Eb.inverse(err);
1922 Ea[0][0] = Ec[0][0];
1923 Ea[0][1] = Ec[0][1];
1924 Ea[0][2] = Ec[0][2];
1925 Ea[0][3] = Ec[0][3];
1926 Ea[1][1] = Ec[1][1];
1927 Ea[1][2] = Ec[1][2];
1928 Ea[1][3] = Ec[1][3];
1929 Ea[2][2] = Ec[2][2];
1930 Ea[2][3] = Ec[2][3];
1931 Ea[3][3] = Ec[3][3];
1932 }
1933 else {
1934 dim = 6;
1935 Ea = d2chi2d2a.inverse(err);
1936 // std::cout << "err flg=" << err << std::endl;
1937 }
1938
1939
1940 //...Store information...
1941 if (! err) {
1942 for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
1943 SymMatrix Ea_5dim(5, 0);
1944 Ea_5dim = Ea.sub(1, 5);
1945 t._helix->a(a_5dim);
1946 t._helix->Ea(Ea_5dim);
1947 tev = a[5];
1948 tev_err = sqrt(Ea[5][5]);
1949 //temp
1950 // std::cout << "nTrial=" << nTrial << std::endl;
1951 // std::cout << "chi2=" << chi2 << std::endl;
1952 // std::cout << "tev,tev_err="<<tev<<" "<<tev_err<<std::endl;
1953 //temp
1954
1955 t._fitted = true;
1956 }
1957 else {
1958 err = TFitFailed;
1959 }
1960
1961 t._ndf = nCores - dim;
1962 t._chi2 = chi2;
1963
1964 if(pre_chi2)*pre_chi2 = _pre_chi2;
1965 if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
1966
1967 return err;
1968}
1969#endif
1970
1971//=========================================
1972void
1973THelixFitter::drift(const TTrack & t,
1974 TMLink & l,
1975 float tev,
1976 double & distance,
1977 double & err,
1978 double & dddt) const {
1979//=========================================
1980// be cautious here
1981 const TMDCWireHit & h = * l.hit();
1982 const HepPoint3D & onTrack = l.positionOnTrack();
1983 const HepPoint3D & onWire = l.positionOnWire();
1984 unsigned leftRight = WireHitRight;
1985 if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
1986
1987 //...No correction...
1988 if ((tev == 0.) && (! _propagation) && (! _tof)) {
1989 distance = h.drift(leftRight);
1990 err = h.dDrift(leftRight);
1991 return;
1992 }
1993
1994 //...TOF correction...
1995 float tof = 0.;
1996 if (_tof) {
1997 int imass = 3;
1998 float tl = t.helix().a()[4];
1999 float f = sqrt(1. + tl * tl);
2000 float s = fabs(t.helix().curv()) * fabs(l.dPhi()) * f;
2001 float p = f / fabs(t.helix().a()[2]);
2002//zsl calcdc_tof2_(& imass, & p, & s, & tof);
2003 float Mass[5] = {0.000511,0.105,0.140,0.493677,0.98327};
2004 tof = s/29.98*sqrt(1.0+(Mass[imass-1]/p)*(Mass[imass-1]/p));
2005 //cout<<"tof:"<<tof<<endl;
2006 }
2007
2008 //...T0 and propagation corrections...
2009 int wire = h.wire()->localId();
2010 int layerId = h.wire()->layerId();
2011// int wire = h.wire()->id();
2012 int side = leftRight;
2013
2014
2015 /// by wangjk, propagation correction
2016 const double zhit = onWire.z();
2017 const double vinner = 220.0e8; // unit is cm/s
2018 const double vouter = 240.0e8;
2019
2020 double tprop = 0.;
2021 const double vprop = (layerId<8) ? vinner : vouter;
2022 if (0 == layerId%2){
2023 /// readout in the west of MDC
2024 tprop = fabs((zhit - h.wire()->backwardPosition()[2]))/vprop;
2025 }else{
2026 /// readout in the east of MDC
2027 tprop = fabs(( zhit - h.wire()->forwardPosition()[2]))/vprop;
2028 }
2029
2030
2031// if (side==0) side = -1;
2032// HepVector3D tp = t.helix().momentum(l.dPhi());
2033// float p[3] = {tp.x(),tp.y(),tp.z()};
2034// float x[3] = {onWire.x(), onWire.y(), onWire.z()};
2035 float time = h.reccdc()->tdc + tev - tof - 1.0e9*tprop;
2036 // float dist_p;
2037 // float dist_m;
2038// float dist;
2039// float edist;
2040
2041// cout<<"tdc: "<<h.reccdc()->tdc<<" tev: "<<tev<<" tof: "<<tof<<endl;
2042
2043
2044 double EsT0 = -1.;
2045 IMessageSvc *msgSvc;
2046 Gaudi::svcLocator()->service("MessageSvc", msgSvc);
2047 MsgStream log(msgSvc, "TCosmicFitter");
2048
2049 IDataProviderSvc* eventSvc = NULL;
2050 Gaudi::svcLocator()->service("EventDataSvc", eventSvc);
2051
2052 SmartDataPtr<RecEsTimeCol> aevtimeCol (eventSvc,"/Event/Recon/RecEsTimeCol");
2053 if (aevtimeCol) {
2054 RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
2055 EsT0 = (*iter_evt)->getTest();
2056 }else{
2057 log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
2058 }
2059
2060 double rawTime = 0.;
2061 rawTime = h.reccdc()->tdc;
2062 double rawadc = 0.;
2063 rawadc =h.reccdc()->adc;
2064// double timewalk = CalibFunSvc_->getTimeWalk(layerid, 0.0);
2065// double timeDrift = rawTime - tof - tprop - EsTo - toWalk;
2066
2067
2068//----cal drift----//
2069 IMdcCalibFunSvc* l_mdcCalFunSvc;
2070 Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
2071
2072 double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
2073 double drifttime = rawTime - tof - 1.0e9*tprop - EsT0 - timeWalk;
2074
2075 l.setDriftTime(drifttime);
2076
2077
2078 double dist;
2079 double edist;
2080 int prop = _propagation;
2081
2082 // //calculate derivative w.r.t. time in blute force way; need update
2083 // //in future to speed up
2084 // float time_p = time + 0.1;
2085 // calcdc_driftdist_(& prop,
2086 // & wire,
2087 // & side,
2088 // p,
2089 // x,
2090 // & time_p,
2091 // & dist_p,
2092 // & edist);
2093 //
2094 // float time_m = time - 0.1;
2095 // calcdc_driftdist_(& prop,
2096 // & wire,
2097 // & side,
2098 // p,
2099 // x,
2100 // & time_m,
2101 // & dist_m,
2102 // & edist);
2103 ////dddt = (dist_p - dist_m)/0.2;
2104 // dddt = (dist_p - dist_m)*5.;
2105 // std::cout << "side=" << side << std::endl;
2106 // std::cout << "dddt=" << dddt << std::endl;
2107
2108// float dist2[2];
2109// float sigma_d2[2];
2110// float deriv2[2];
2111 float time_tmp = time;
2112
2113 //----cal drift----//
2114// IMdcCalibFunSvc* l_mdcCalFunSvc;
2115 Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
2116 // dist = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_tmp, layerId, wire, side, 0.0);
2117 dist = l_mdcCalFunSvc->driftTimeToDist(time_tmp,layerId, wire, side,0.0);//by liucy 2010/05/12
2118 edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, 0.0);
2119 dist = dist/10.0; //mm->cm
2120 edist = edist/10.0;
2121
2122 distance = dist;
2123 err = edist;
2124
2125// cout<<"dist: "<<dist<<" edist: "<<edist<<endl;
2126
2127 float time_p = time - 0.1;
2128 float time_m = time + 0.1;
2129// float dist_p = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_p, layerId, wire, side, 0.0);
2130// float dist_m = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_m, layerId, wire, side, 0.0);
2131// dist_p = dist_p/10.;
2132// dist_m = dist_m/10.;
2133// dddt = (dist_m - dist_p)/0.2;
2134// dddt = 0;
2135// dddt = 0.004;
2136
2137// float dist2[2] = {dist, dist};
2138// float sigma_d2[2] = {edist, edist};
2139// float deriv2[2] = {dddt_tmp, dddt_tmp}; // cm/ns
2140// float deriv2[2] = {0.00, 0.00};
2141// float deriv2[2] = {0.004, 0.004};
2142//cout<<"dddt_tmp: "<<dddt_tmp<<endl;
2143
2144//zsl calcdc_driftdist3_(& prop,
2145// & wire,
2146// p,
2147// x,
2148// & time_tmp,
2149// dist2,
2150// sigma_d2,
2151// deriv2);
2152 //n.b. input and output time are slightly different because of prop.
2153 //delay corr. in driftdist3.
2154 // calcdc_driftdist_(& prop,
2155 // & wire,
2156 // & side,
2157 // p,
2158 // x,
2159 // & time,
2160 // & dist,
2161 // & edist);
2162
2163/* if (side==-1) {
2164 // std::cout << " " << std::endl;
2165 // std::cout << dist << " " << dist2[0] << " " <<dist2[1] << std::endl;
2166 // std::cout << edist << " " << sigma_d2[0] << " " << sigma_d2[1] << std::endl;
2167 // std::cout << dddt << " " << 0.001*deriv2[0] << std::endl;
2168 dist = dist2[0];
2169 edist = sigma_d2[0];
2170 //dddt = 0.001*deriv2[0];
2171 dddt = deriv2[0];
2172 } else if(side== 1) {
2173 // std::cout << " " << std::endl;
2174 // std::cout << dist << " " << dist2[1] << " " <<dist2[0] << std::endl;
2175 // std::cout << edist << " " << sigma_d2[1] << " " << sigma_d2[0] << std::endl;
2176 // std::cout << dddt << " " << 0.001*deriv2[1] << std::endl;
2177 dist = dist2[1];
2178 edist = sigma_d2[1];
2179 //dddt = 0.001*deriv2[1];
2180 dddt = deriv2[1];
2181 }
2182
2183 distance = (double) dist;
2184// std::cout << "time,distance,dddt="<<time<<" "<<distance<<" "<<dddt<<std::endl;
2185 err = (double) edist;
2186*/
2187 return;
2188}
2189
2190
2191//add by jialk, to return drift time after prop correction
2192/*
2193//=========================================
2194void
2195THelixFitter::drift(const TTrack & t,
2196 const TMLink & l,
2197 float tev,
2198 double & distance,
2199 double & err,
2200 double & dddt) const {
2201//=========================================
2202*/
sin
double sin(const BesAngle a)
Definition: BesAngle.h:210
cos
double cos(const BesAngle a)
Definition: BesAngle.h:213
x
Double_t x[10]
Definition: DataBase/tau_mode.c:57
time
Double_t time
Definition: DataBase/tau_mode.c:7
alpha
const double alpha
Definition: FastVertexFit.cxx:4
HepPoint3D
HepGeom::Point3D< double > HepPoint3D
Definition: Gam4pikp.cxx:37
s
XmlRpcServer s
Definition: HelloServer.cpp:11
DBL_MAX
#define DBL_MAX
Definition: KalFitAlg.h:13
v
**********Class see also m_nmax DOUBLE PRECISION m_amel DOUBLE PRECISION m_x2 DOUBLE PRECISION m_alfinv DOUBLE PRECISION m_Xenph INTEGER m_KeyWtm INTEGER m_idyfs DOUBLE PRECISION m_zini DOUBLE PRECISION m_q2 DOUBLE PRECISION m_Wt_KF DOUBLE PRECISION m_WtCut INTEGER m_KFfin *COMMON c_KarLud $ !Input CMS energy[GeV] $ !CMS energy after beam spread beam strahlung[GeV] $ !Beam energy spread[GeV] $ !z boost due to beam spread $ !electron beam mass *ff pair spectrum $ !minimum v
Definition: KarLud.h:35
msgSvc
IMessageSvc * msgSvc()
Definition: ServiceAccessor.h:13
Convergence
#define Convergence
Definition: TCosmicFitter.cxx:76
NTrailMax
#define NTrailMax
Definition: TCosmicFitter.cxx:75
TrkRecoHelixFitterChisqMax
float TrkRecoHelixFitterChisqMax
Definition: TrkReco.cxx:78
WireHitLeft
#define WireHitLeft
Definition: TMDCWireHit.h:21
WireHitRight
#define WireHitRight
Definition: TMDCWireHit.h:22
WireHitPatternLeft
#define WireHitPatternLeft
Definition: TMDCWireHit.h:33
WireHitPatternRight
#define WireHitPatternRight
Definition: TMDCWireHit.h:34
TFitAlreadyFitted
#define TFitAlreadyFitted
Definition: TMFitter.h:28
TFitFailed
#define TFitFailed
Definition: TMFitter.h:30
TFitUnavailable
#define TFitUnavailable
Definition: TMFitter.h:31
TFitErrorFewHits
#define TFitErrorFewHits
Definition: TMFitter.h:29
Track
#define Track
Definition: TTrackBase.h:30
Helix::a
const HepVector & a(void) const
returns helix parameters.
Definition: Reconstruction/TrackUtil/TrackUtil-00-00-08/TrackUtil/Helix.h:263
IMagneticFieldSvc::getReferField
virtual double getReferField()=0
IMdcCalibFunSvc::driftTimeToDist
virtual double driftTimeToDist(double drifttime, int layid, int cellid, int lr, double entrance=0.0) const =0
IMdcCalibFunSvc::getSigma
virtual double getSigma(int layid, int lr, double dist, double entrance=0.0, double tanlam=0.0, double z=0.0, double Q=1000.0) const =0
IMdcCalibFunSvc::getTprop
virtual double getTprop(int lay, double z) const =0
IMdcCalibFunSvc::getT0
virtual double getT0(int layid, int cellid) const =0
IMdcCalibFunSvc::getTimeWalk
virtual double getTimeWalk(int layid, double Q) const =0
MdcRec_wirhit::adc
float adc
Definition: MdcTables.h:340
MdcRec_wirhit::tdc
float tdc
Definition: MdcTables.h:339
THelixFitter::chi2
double chi2(void) const
returns sum of chi2 aftter fit.
Definition: THelixFitter.h:305
THelixFitter::~THelixFitter
virtual ~THelixFitter()
Destructor.
Definition: THelixFitter.cxx:144
THelixFitter::tof
bool tof(void) const
sets/returns propagation-delay correction flag.
Definition: THelixFitter.h:207
THelixFitter::THelixFitter
THelixFitter(const std::string &name)
Constructor.
Definition: THelixFitter.cxx:122
THelixFitter::tanl
bool tanl(void) const
sets/returns tanLambda correction flag.
Definition: THelixFitter.h:219
TMDCWireHit::reccdc
struct MdcRec_wirhit * reccdc(void) const
returns a pointer to RECMDC_WIRHIT.
Definition: TMDCWireHit.h:224
TMDCWireHit::dump
void dump(const std::string &message=std::string(""), const std::string &prefix=std::string("")) const
dumps debug information.
Definition: TMDCWireHit.cxx:64
TMDCWireHit::state
unsigned state(void) const
returns state.
Definition: TMDCWireHit.h:230
TMDCWireHit::dDrift
float dDrift(unsigned) const
returns drift distance error.
Definition: TMDCWireHit.h:243
TMDCWireHit::drift
float drift(unsigned) const
returns drift distance.
Definition: TMDCWireHit.h:236
TMDCWireHit::wire
const TMDCWire *const wire(void) const
returns a pointer to a TMDCWire.
Definition: TMDCWireHit.h:218
TMDCWire
A class to represent a wire in MDC.
Definition: TMDCWire.h:55
TMDCWire::localId
unsigned localId(void) const
returns local id in a wire layer.
Definition: TMDCWire.h:213
TMDCWire::layerId
unsigned layerId(void) const
returns layer id.
Definition: TMDCWire.h:219
TMDCWire::direction
const HepVector3D & direction(void) const
returns direction vector of the wire.
Definition: TMDCWire.h:342
TMDCWire::xyPosition
const HepPoint3D & xyPosition(void) const
returns middle position of a wire. z componet is 0.
Definition: TMDCWire.h:327
TMDCWire::forwardPosition
const HepPoint3D & forwardPosition(void) const
returns position in forward endplate.
Definition: TMDCWire.h:300
TMDCWire::backwardPosition
const HepPoint3D & backwardPosition(void) const
returns position in backward endplate.
Definition: TMDCWire.h:306
TMDCWire::name
std::string name(void) const
returns name.
Definition: TMDCWire.h:412
TMDCWire::wirePosition
void wirePosition(float zPosition, HepPoint3D &xyPosition, HepPoint3D &backwardPosition, HepVector3D &direction) const
calculates position and direction vector with sag correction.
Definition: TMDCWire.cxx:541
TMFitter
A class to fit a TTrackBase object.
Definition: TMFitter.h:34
TTrackBase
A virtual class for a track class in tracking.
Definition: TTrackBase.h:46
TTrackBase::objectType
virtual unsigned objectType(void) const
returns object type.
Definition: TTrackBase.h:268
TTrack
A class to represent a track in tracking.
Definition: TTrack.h:129
t
int t()
Definition: t.c:1
pi
const float pi
Definition: vector3.h:133