10#include <klm/bklm/modules/bklmTracking/BKLMTrackFitter.h>
13#include <klm/bklm/geometry/GeometryPar.h>
14#include <klm/bklm/geometry/Module.h>
17#include <framework/logging/Logger.h>
20#include <CLHEP/Matrix/DiagMatrix.h>
21#include <CLHEP/Matrix/Matrix.h>
28using namespace Belle2::bklm;
31enum { VX = 0, VY = 1, VZ = 2 };
34enum { AY = 0, BY = 1, AZ = 2, BZ = 3 };
37enum { MY = 0, MZ = 1 };
64 if (listHitSector.size() < 2) {
71 HepSymMatrix error(2, 0);
72 HepVector gloEta(2, 0);
73 HepSymMatrix gloError(2, 0);
77 HepVector sectorPar(4, 0);
78 HepSymMatrix sectorErr(4, 0);
79 HepVector globalPar(4, 0);
80 HepSymMatrix globalErr(4, 0);
84 globalPar.sub(1, eta);
85 globalErr.sub(1, error);
88 sectorPar.sub(1, eta);
89 sectorErr.sub(1, error);
94 globalPar.sub(3, eta);
95 globalErr.sub(3, error);
98 sectorPar.sub(3, eta);
99 sectorErr.sub(3, error);
105 (*listHitSector.begin())->getSector(), 1);
107 Hep3Vector p1(0, 0, 0); Hep3Vector p2(0, 0, 0);
110 double y1 = sectorPar[0] + sectorPar[1] * x1;
111 double y2 = sectorPar[0] + sectorPar[1] * x2;
112 double z1 = sectorPar[2] + sectorPar[3] * x1;
113 double z2 = sectorPar[2] + sectorPar[3] * x2;
114 p1.setX(x1); p1.setY(y1); p1.setZ(z1);
115 p2.setX(x2); p2.setY(y2); p2.setZ(z2);
120 if (gl2[0] != gl1[0]) {
121 globalPar[1] = (gl2[1] - gl1[1]) / (gl2[0] - gl1[0]);
122 globalPar[0] = gl1[1] - globalPar[1] * gl1[0];
123 globalPar[3] = (gl2[2] - gl1[2]) / (gl2[0] - gl1[0]);
124 globalPar[2] = gl1[2] - globalPar[3] * gl1[0];
125 globalErr = sectorErr;
127 globalPar[1] = DBL_MAX;
128 globalPar[0] = DBL_MAX;
129 globalPar[3] = DBL_MAX;
130 globalPar[2] = DBL_MAX;
131 globalErr = sectorErr;
160 double x, y, z, dy, dz;
172 CLHEP::Hep3Vector globalPos(hit->getPositionX(), hit->getPositionY(), hit->getPositionZ());
185 double distance =
sqrt(dy * dy + dz * dz);
191 HepMatrix errors(2, 2, 0);
192 HepMatrix A(2, 4, 0);
207 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
208 double dn = nStrips - 1.5;
209 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
210 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
212 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
214 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
215 hit_localZErr = hit_localZErr *
sqrt(factor);
218 error =
sqrt(errors[ MY ][ MY ] +
220 pow(hit_localPhiErr, 2) +
221 pow(hit_localZErr, 2));
224 sigma = distance / error;
245 double x_mea = hit->getPositionX();
246 double y_mea = hit->getPositionY();
247 double z_mea = hit->getPositionZ();
252 double dx = x_pre - x_mea;
253 double dz = z_pre - z_mea;
255 double distance =
sqrt(dx * dx + dz * dz);
259 HepMatrix errors(2, 2, 0);
260 HepMatrix A(2, 4, 0);
278 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
279 double dn = nStrips - 1.5;
280 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
281 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
283 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
285 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
286 hit_localZErr = hit_localZErr *
sqrt(factor);
290 double sinphi = globalOrigin[1] / globalOrigin.mag();
291 double cosphi = globalOrigin[0] / globalOrigin.mag();
293 HepMatrix globalHitErr(3, 3, 0);
294 globalHitErr[0][0] = pow(hit_localPhiErr * sinphi, 2);
295 globalHitErr[0][1] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
296 globalHitErr[0][2] = 0;
297 globalHitErr[1][1] = pow(hit_localPhiErr * cosphi, 2);;
298 globalHitErr[1][0] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
299 globalHitErr[1][2] = 0;
300 globalHitErr[2][2] = pow(hit_localZErr, 2);
301 globalHitErr[2][0] = 0;
302 globalHitErr[2][1] = 0;
312 HepMatrix error_mea(2, 2, 0);
313 error =
sqrt(errors[ MY ][ MY ] +
321 sigma = distance / error;
334 int depDir,
int indDir)
338 Hep3Vector localHitPos;
339 HepMatrix localHitErr(3, 3, 0);
346 int noPoints = hitList.size();
349 HepMatrix A(noPoints, 2, 0);
352 HepVector y(noPoints, 0);
356 HepDiagMatrix V_y_inverse(noPoints, 0);
360 HepSymMatrix V_A, V_A_inverse;
362 int section = (*hitList.begin())->getSection();
363 int sector = (*hitList.begin())->getSector();
371 if (hit->getSection() != section || hit->getSector() != sector) {
379 CLHEP::Hep3Vector globalPos;
380 globalPos[0] = hit->getPositionX();
381 globalPos[1] = hit->getPositionY();
382 globalPos[2] = hit->getPositionZ();
390 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
391 double dn = nStrips - 1.5;
392 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
393 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
395 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
397 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
398 hit_localZErr = hit_localZErr *
sqrt(factor);
401 localHitErr[0][0] = 0.0;
402 localHitErr[0][1] = 0;
403 localHitErr[0][2] = 0;
404 localHitErr[1][1] = hit_localPhiErr;
405 localHitErr[1][0] = 0;
406 localHitErr[1][2] = 0;
407 localHitErr[2][2] = hit_localZErr;
408 localHitErr[2][0] = 0;
409 localHitErr[2][1] = 0;
414 indPos = localHitPos.x();
418 indPos = localHitPos.y();
422 indPos = localHitPos.z();
426 B2DEBUG(20,
"error in klm_trackSectorFit: illegal direction");
433 depPos = localHitPos.x();
437 depPos = localHitPos.y();
441 depPos = localHitPos.z();
445 B2DEBUG(20,
"error in klm_trackSectorFit: illegal direction");
451 A[ n ][ 1 ] = indPos;
455 if (localHitErr[ depDir ][ depDir ] > 0.0) {
456 V_y_inverse[ n ][ n ] = 1.0 / localHitErr[ depDir ][ depDir ];
458 V_y_inverse[ n ][ n ] = DBL_MAX;
463 V_A_inverse = V_y_inverse.similarityT(A);
466 V_A = V_A_inverse.inverse(ierr);
468 eta = V_A * A.T() * V_y_inverse * y;
472 HepMatrix residual = y - A * eta;
473 HepMatrix chisqr = residual.T() * V_y_inverse * residual;
475 return (chisqr.trace());
483 int depDir,
int indDir)
488 HepMatrix globalHitErr(3, 3, 0);
494 int noPoints = hitList.size();
497 HepMatrix A(noPoints, 2, 0);
500 HepVector y(noPoints, 0);
504 HepDiagMatrix V_y_inverse(noPoints, 0);
507 HepSymMatrix V_A, V_A_inverse;
519 CLHEP::Hep3Vector globalPos;
520 globalPos[0] = hit->getPositionX();
521 globalPos[1] = hit->getPositionY();
522 globalPos[2] = hit->getPositionZ();
530 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
531 double dn = nStrips - 1.5;
532 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
533 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
535 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
537 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
538 hit_localZErr = hit_localZErr *
sqrt(factor);
542 double sinphi = globalOrigin[1] / globalOrigin.mag();
543 double cosphi = globalOrigin[0] / globalOrigin.mag();
545 globalHitErr[0][0] = pow(hit_localPhiErr * sinphi, 2);
546 globalHitErr[0][1] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
547 globalHitErr[0][2] = 0;
548 globalHitErr[1][1] = pow(hit_localPhiErr * cosphi, 2);;
549 globalHitErr[1][0] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
550 globalHitErr[1][2] = 0;
551 globalHitErr[2][2] = pow(hit_localZErr, 2);;
552 globalHitErr[2][0] = 0;
553 globalHitErr[2][1] = 0;
558 indPos = globalPos.x();
562 indPos = globalPos.y();
566 indPos = globalPos.z();
570 B2DEBUG(20,
"error in bklm trackFit: illegal direction");
577 depPos = globalPos.x();
581 depPos = globalPos.y();
585 depPos = globalPos.z();
589 B2DEBUG(20,
"error in bklm trackFit: illegal direction");
595 A[ n ][ 1 ] = indPos;
599 double error_raw = globalHitErr[indDir][indDir] + globalHitErr[depDir][depDir];
602 double weight = error_raw;
604 V_y_inverse[ n ][ n ] = 1.0 / weight;
606 V_y_inverse[ n ][ n ] = DBL_MAX;
614 V_A_inverse = V_y_inverse.similarityT(A);
617 V_A = V_A_inverse.inverse(ierr);
619 eta = V_A * A.T() * V_y_inverse * y;
623 HepMatrix residual = y - A * eta;
624 HepMatrix chisqr = residual.T() * V_y_inverse * residual;
626 return (chisqr.trace());
double fit(std::list< KLMHit2d * > &listTrackPoint)
do fit and returns chi square of the fit.
float m_Chi2
Chi square of fit.
double fit1dTrack(std::list< KLMHit2d * > hitList, CLHEP::HepVector &eta, CLHEP::HepSymMatrix &error, int depDir, int indDir)
do fit in the global system
int m_NumHit
the number of hits on this track
double globalDistanceToHit(KLMHit2d *hit, double &error, double &sigma)
Distance from track to a hit in the global system.
BKLMTrackFitter()
Default constructor.
double distanceToHit(KLMHit2d *hit, double &error, double &sigma)
Distance from track to a hit in the plane of the module.
CLHEP::HepSymMatrix m_GlobalErr
track params errors in global system
CLHEP::HepVector m_GlobalPar
track params in global system
bklm::GeometryPar * m_GeoPar
pointer to GeometryPar singleton
double fit1dSectorTrack(std::list< KLMHit2d * > hitList, CLHEP::HepVector &eta, CLHEP::HepSymMatrix &error, int depDir, int indDir)
do fit in the y-x plane or z-x plane
CLHEP::HepVector m_SectorPar
track params in the sector local system
CLHEP::HepSymMatrix m_SectorErr
track params errors in the sector local system
bool m_Valid
Is fit valid.
bool m_globalFit
do fit in the local system or global system false: local sys; true: global sys.
~BKLMTrackFitter()
Destructor.
const Module * findModule(int section, int sector, int layer) const
Get the pointer to the definition of a module.
static GeometryPar * instance(void)
Static method to get a reference to the singleton GeometryPar instance.
Define the geometry of a BKLM module Each sector [octant] contains Modules.
const CLHEP::Hep3Vector globalToLocal(const CLHEP::Hep3Vector &v, bool reco=false) const
Transform space-point within this module from global to local coordinates.
double getPhiStripWidth() const
Get phi-strip width.
const CLHEP::Hep3Vector getGlobalOrigin() const
Return the position (in global coordinates) of this module's sensitive-volume origin.
const CLHEP::Hep3Vector localToGlobal(const CLHEP::Hep3Vector &v, bool reco=false) const
Transform space-point within this module from local to global coordinates.
double getZStripWidth() const
Get z-strip width.
double sqrt(double a)
sqrt for double
Abstract base class for different kinds of events.