00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #ifdef TRKRECO_DEBUG_DETAIL
00014 #ifndef TRKRECO_DEBUG
00015 #define TRKRECO_DEBUG
00016 #endif
00017 #endif
00018 #include <iostream>
00019 #include <float.h>
00020 #include "TrkReco/T3DLineFitter.h"
00021 #include "TrkReco/T3DLine.h"
00022 #define HEP_SHORT_NAMES
00023
00024
00025 #include "MdcTables/MdcTables.h"
00026 #include "CLHEP/Matrix/Vector.h"
00027 #include "CLHEP/Matrix/SymMatrix.h"
00028 #include "CLHEP/Matrix/Matrix.h"
00029 #include "TrkReco/TMLink.h"
00030 #include "TrkReco/TTrackBase.h"
00031
00032 #include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
00033 #include "MdcCalibFunSvc/MdcCalibFunSvc.h"
00034
00035 #include "GaudiKernel/MsgStream.h"
00036 #include "GaudiKernel/AlgFactory.h"
00037 #include "GaudiKernel/ISvcLocator.h"
00038 #include "GaudiKernel/IMessageSvc.h"
00039 #include "GaudiKernel/IDataProviderSvc.h"
00040 #include "GaudiKernel/IDataManagerSvc.h"
00041 #include "GaudiKernel/SmartDataPtr.h"
00042 #include "GaudiKernel/IDataProviderSvc.h"
00043 #include "GaudiKernel/PropertyMgr.h"
00044 #include "GaudiKernel/IJobOptionsSvc.h"
00045 #include "GaudiKernel/IMessageSvc.h"
00046 #include "GaudiKernel/Bootstrap.h"
00047 #include "GaudiKernel/StatusCode.h"
00048
00049 #include "GaudiKernel/ContainedObject.h"
00050 #include "GaudiKernel/SmartRef.h"
00051 #include "GaudiKernel/SmartRefVector.h"
00052 #include "GaudiKernel/ObjectVector.h"
00053 #include "EventModel/EventModel.h"
00054
00055 #include "EvTimeEvent/RecEsTime.h"
00056
00057 using CLHEP::HepVector;
00058 using CLHEP::HepSymMatrix;
00059 using CLHEP::HepMatrix;
00060
00061 #define T3DLine2DFit 2
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080 T3DLineFitter::T3DLineFitter(const std::string& name)
00081 : TMFitter(name),
00082
00083 _sag(false),_propagation(0),_tof(true){
00084
00085 }
00086 T3DLineFitter::T3DLineFitter(const std::string& name,
00087 bool m_sag,int m_prop,bool m_tof)
00088 : TMFitter(name),
00089 _sag(m_sag),_propagation(m_prop),_tof(m_tof){
00090 }
00091
00092 T3DLineFitter::~T3DLineFitter() {
00093 }
00094
00095 void T3DLineFitter::sag(bool _in){
00096 _sag = _in;
00097 }
00098 void T3DLineFitter::propagation(int _in){
00099 _propagation = _in;
00100 }
00101 void T3DLineFitter::tof(bool _in){
00102 _tof = _in;
00103 }
00104
00105 void T3DLineFitter::drift(const T3DLine& t,const TMLink& l,
00106 float t0Offset,
00107 double& distance,double& err) const{
00108
00109
00110 double _t0_bes = -1.;
00111 IMessageSvc *msgSvc;
00112 Gaudi::svcLocator()->service("MessageSvc", msgSvc);
00113 MsgStream log(msgSvc, "TCosmicFitter");
00114
00115 IDataProviderSvc* eventSvc = NULL;
00116 Gaudi::svcLocator()->service("EventDataSvc", eventSvc);
00117
00118 SmartDataPtr<RecEsTimeCol> aevtimeCol(eventSvc,"/Event/Recon/RecEsTimeCol");
00119 if (aevtimeCol) {
00120 RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
00121 _t0_bes = (*iter_evt)->getTest();
00122 }else{
00123 log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
00124 }
00125
00126
00127 const TMDCWireHit& h = *l.hit();
00128 const HepPoint3D& onTrack = l.positionOnTrack();
00129 const HepPoint3D& onWire = l.positionOnWire();
00130 unsigned leftRight = WireHitRight;
00131
00132 if((onWire.x()*onTrack.y()-onWire.y()*onTrack.x())<0) leftRight = WireHitLeft;
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143 float tof = 0.;
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166 double Tfly = _t0_bes/220.*(110.-onWire.y());
00167
00168
00169
00170 int wire = h.wire()->localId();
00171 int layerId = h.wire()->layerId();
00172
00173 int side = leftRight;
00174
00175
00176
00177
00178 float time = h.reccdc()->tdc - Tfly;
00179 float dist;
00180 float edist;
00181 IMdcCalibFunSvc* l_mdcCalFunSvc;
00182 Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
00183 double T0 = l_mdcCalFunSvc->getT0(layerId,wire);
00184 double rawadc = 0.;
00185 rawadc =h.reccdc()->adc;
00186
00187 double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
00188
00189 double drifttime =time -T0-timeWalk;
00190
00191 dist = l_mdcCalFunSvc->driftTimeToDist(drifttime,layerId, wire, side, 0.0);
00192 edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, 0.0);
00193
00194 dist = dist/10.0;
00195 edist = edist/10.0;
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206 distance = (double) dist;
00207 err = (double) edist;
00208 return;
00209 }
00210
00211 int T3DLineFitter::fit(TTrackBase& tb) const{
00212 return fit(tb,0);
00213 }
00214
00215 int T3DLineFitter::fit(TTrackBase& tb, float t0Offset) const{
00216
00217
00218
00219
00220 if(tb.objectType() != Line3D) return TFitUnavailable;
00221 T3DLine& t = (T3DLine&) tb;
00222
00223
00224 if(t.fitted()) return TFitAlreadyFitted;
00225
00226
00227 AList<TMLink> cores = t.cores();
00228 unsigned nCores = cores.length();
00229 unsigned nStereoCores = NStereoHits(cores);
00230
00231
00232
00233 bool flag2D = false;
00234 if ((nStereoCores == 0) && (nCores > 3)) flag2D = true;
00235 else if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
00236 return TFitErrorFewHits;
00237
00238
00239 const HepPoint3D pivot_bak = t.pivot();
00240 t.pivot(ORIGIN);
00241
00242
00243 Vector a(4),da(4);
00244 a = t.a();
00245 Vector dxda(4);
00246 Vector dyda(4);
00247 Vector dzda(4);
00248 Vector dDda(4);
00249 Vector dchi2da(4);
00250 SymMatrix d2chi2d2a(4,0);
00251 static const SymMatrix zero4(4,0);
00252 double chi2;
00253 double chi2Old = DBL_MAX;
00254 double factor = 1.0;
00255 int err = 0;
00256 SymMatrix e(2,0);
00257 Vector f(2);
00258
00259
00260 unsigned nTrial = 0;
00261 while(nTrial < 100){
00262
00263
00264 chi2 = 0;
00265 for (unsigned j=0;j<4;j++) dchi2da[j]=0;
00266 d2chi2d2a=zero4;
00267
00268
00269 unsigned i=0;
00270 while(TMLink* l = cores[i++]){
00271 const TMDCWireHit& h = *l->hit();
00272
00273
00274 t.approach(*l,_sag);
00275 const HepPoint3D& onTrack=l->positionOnTrack();
00276 const HepPoint3D& onWire=l->positionOnWire();
00277 unsigned leftRight = WireHitRight;
00278 if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
00279
00280
00281 double distance;
00282 double eDistance;
00283 drift(t, * l, t0Offset, distance, eDistance);
00284 l->drift(distance,0);
00285 l->drift(distance,1);
00286 l->dDrift(eDistance,0);
00287 l->dDrift(eDistance,1);
00288 double eDistance2 = eDistance * eDistance;
00289
00290
00291
00292 HepVector3D v = onTrack - onWire;
00293 double vmag = v.mag();
00294 double dDistance = vmag - distance;
00295
00296 HepVector3D vw;
00297
00298 this->dxda(*l, t, dxda, dyda, dzda, vw);
00299
00300
00301 dDda = (vmag > 0.)
00302 ? ((v.x() * (1. - vw.x() * vw.x()) -
00303 v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
00304 * dxda +
00305 (v.y() * (1. - vw.y() * vw.y()) -
00306 v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
00307 * dyda +
00308 (v.z() * (1. - vw.z() * vw.z()) -
00309 v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
00310 * dzda) / vmag : Vector(4,0);
00311 if (vmag <= 0.0) {
00312 std::cout << " in fit " << onTrack << ", " << onWire;
00313 h.dump();
00314 }
00315 dchi2da += (dDistance / eDistance2) * dDda;
00316 d2chi2d2a += vT_times_v(dDda) / eDistance2;
00317 double pChi2 = dDistance * dDistance / eDistance2;
00318 chi2 += pChi2;
00319
00320
00321 l->update(onTrack, onWire, leftRight, pChi2);
00322 }
00323
00324
00325 double change = chi2Old - chi2;
00326
00327 if (fabs(change) < 1.0e-5) break;
00328 if (change < 0.) {
00329 a += factor * da;
00330 factor *= 0.1;
00331 }else{
00332 chi2Old = chi2;
00333 if(flag2D){
00334 f = dchi2da.sub(1,2);
00335 e = d2chi2d2a.sub(1,2);
00336 f = solve(e,f);
00337 da[0]=f[0];
00338 da[1]=f[1];
00339 da[2]= 0;
00340 da[3]= 0;
00341 }else{
00342
00343 da = solve(d2chi2d2a, dchi2da);
00344 }
00345 }
00346 a -= factor * da;
00347 t.a(a);
00348 ++nTrial;
00349 }
00350
00351
00352 SymMatrix Ea(4,0);
00353 unsigned dim;
00354 if(flag2D){
00355 dim=2;
00356 SymMatrix Eb(3,0),Ec(3,0);
00357 Eb = d2chi2d2a.sub(1,3);
00358 Ec = Eb.inverse(err);
00359 Ea[0][0] = Ec[0][0];
00360 Ea[0][1] = Ec[0][1];
00361 Ea[0][2] = Ec[0][2];
00362 Ea[1][1] = Ec[1][1];
00363 Ea[1][2] = Ec[1][2];
00364 Ea[2][2] = Ec[2][2];
00365 }else{
00366 dim=4;
00367 Ea = d2chi2d2a.inverse(err);
00368 }
00369
00370
00371 if(! err){
00372 t.a(a);
00373 t.Ea(Ea);
00374 t._fitted = true;
00375 if (flag2D) err = T3DLine2DFit;
00376 }else{
00377 err = TFitFailed;
00378 }
00379
00380 t._ndf = nCores - dim;
00381 t._chi2 = chi2;
00382
00383
00384 t.pivot(pivot_bak);
00385
00386 return err;
00387 }
00388
00389 int T3DLineFitter::dxda(const TMLink& l,const T3DLine& t,
00390 Vector& dxda,Vector& dyda,Vector& dzda,
00391 HepVector3D& wireDirection) const{
00392
00393
00394
00395 const TMDCWire& w = *l.wire();
00396 const HepVector3D k = t.k();
00397 const double cosPhi0 = t.cosPhi0();
00398 const double sinPhi0 = t.sinPhi0();
00399 const double dr = t.dr();
00400 const HepPoint3D& onWire = l.positionOnWire();
00401 const HepPoint3D& onTrack = l.positionOnTrack();
00402
00403 const double t_t = (onWire - t.x0()).dot(k)/k.mag2();
00404
00405
00406 HepPoint3D xw = w.xyPosition();
00407 HepPoint3D wireBackwardPosition = w.backwardPosition();
00408 wireDirection = w.direction();
00409 if (_sag) w.wirePosition(onWire.z(),
00410 xw,
00411 wireBackwardPosition,
00412 (HepVector3D&)wireDirection);
00413 const HepVector3D& v = wireDirection;
00414
00415
00416
00417
00418 const double v_k = v.dot(k);
00419 const double tvk = v_k*v_k-k.mag2();
00420 if(tvk==0) return(-1);
00421
00422 const HepVector3D& dxdt_a = k;
00423
00424 const HepVector3D dxda_t[4]
00425 ={HepVector3D(cosPhi0,sinPhi0,0),
00426 HepVector3D(-dr*sinPhi0-t_t*cosPhi0,dr*cosPhi0-t_t*sinPhi0,0),
00427 HepVector3D(0,0,1),
00428 HepVector3D(0,0,t_t)};
00429
00430 const HepVector3D dx0da[4]
00431 ={HepVector3D(cosPhi0,sinPhi0,0),
00432 HepVector3D(-dr*sinPhi0,dr*cosPhi0,0),
00433 HepVector3D(0,0,1),
00434 HepVector3D(0,0,0)};
00435
00436 const HepVector3D dkda[4]
00437 ={HepVector3D(0,0,0),
00438 HepVector3D(-cosPhi0,-sinPhi0,0),
00439 HepVector3D(0,0,0),
00440 HepVector3D(0,0,1)};
00441
00442 const HepVector3D d = t.x0() - wireBackwardPosition;
00443 const HepVector3D kvkv = k - v_k*v;
00444
00445 for(int i=0;i<4;i++){
00446 const double v_dkda = v.dot(dkda[i]);
00447
00448 const double dtda = dx0da[i].dot(kvkv)/tvk
00449 + d.dot(dkda[i]-v_dkda*v)/tvk
00450 - d.dot(kvkv)*2*(v_k*v_dkda-k.dot(dkda[i]))/(tvk*tvk);
00451
00452 const HepVector3D dxda3D = dxda_t[i] + dtda*dxdt_a;
00453
00454 dxda[i] = dxda3D.x();
00455 dyda[i] = dxda3D.y();
00456 dzda[i] = dxda3D.z();
00457 }
00458
00459 return 0;
00460 }