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