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>
18#include <framework/utilities/MathHelpers.h>
21#include <CLHEP/Matrix/DiagMatrix.h>
22#include <CLHEP/Matrix/Matrix.h>
29using namespace Belle2::bklm;
32enum { VX = 0, VY = 1, VZ = 2 };
35enum { AY = 0, BY = 1, AZ = 2, BZ = 3 };
38enum { MY = 0, MZ = 1 };
65 if (listHitSector.size() < 2) {
72 HepSymMatrix error(2, 0);
73 HepVector gloEta(2, 0);
74 HepSymMatrix gloError(2, 0);
78 HepVector sectorPar(4, 0);
79 HepSymMatrix sectorErr(4, 0);
80 HepVector globalPar(4, 0);
81 HepSymMatrix globalErr(4, 0);
85 globalPar.sub(1, eta);
86 globalErr.sub(1, error);
89 sectorPar.sub(1, eta);
90 sectorErr.sub(1, error);
95 globalPar.sub(3, eta);
96 globalErr.sub(3, error);
99 sectorPar.sub(3, eta);
100 sectorErr.sub(3, error);
106 (*listHitSector.begin())->getSector(), 1);
108 Hep3Vector p1(0, 0, 0); Hep3Vector p2(0, 0, 0);
111 double y1 = sectorPar[0] + sectorPar[1] * x1;
112 double y2 = sectorPar[0] + sectorPar[1] * x2;
113 double z1 = sectorPar[2] + sectorPar[3] * x1;
114 double z2 = sectorPar[2] + sectorPar[3] * x2;
115 p1.setX(x1); p1.setY(y1); p1.setZ(z1);
116 p2.setX(x2); p2.setY(y2); p2.setZ(z2);
121 if (gl2[0] != gl1[0]) {
122 globalPar[1] = (gl2[1] - gl1[1]) / (gl2[0] - gl1[0]);
123 globalPar[0] = gl1[1] - globalPar[1] * gl1[0];
124 globalPar[3] = (gl2[2] - gl1[2]) / (gl2[0] - gl1[0]);
125 globalPar[2] = gl1[2] - globalPar[3] * gl1[0];
126 globalErr = sectorErr;
128 globalPar[1] = DBL_MAX;
129 globalPar[0] = DBL_MAX;
130 globalPar[3] = DBL_MAX;
131 globalPar[2] = DBL_MAX;
132 globalErr = sectorErr;
161 double x, y, z, dy, dz;
186 double distance =
sqrt(dy * dy + dz * dz);
192 HepMatrix errors(2, 2, 0);
193 HepMatrix A(2, 4, 0);
209 double dn = nStrips - 1.5;
210 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
211 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
215 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
216 hit_localZErr = hit_localZErr *
sqrt(factor);
219 error =
sqrt(errors[ MY ][ MY ] +
225 sigma = distance / error;
253 double dx = x_pre - x_mea;
254 double dz = z_pre - z_mea;
256 double distance =
sqrt(dx * dx + dz * dz);
260 HepMatrix errors(2, 2, 0);
261 HepMatrix A(2, 4, 0);
280 double dn = nStrips - 1.5;
281 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
282 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
286 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
287 hit_localZErr = hit_localZErr *
sqrt(factor);
291 double sinphi = globalOrigin[1] / globalOrigin.mag();
292 double cosphi = globalOrigin[0] / globalOrigin.mag();
294 HepMatrix globalHitErr(3, 3, 0);
295 globalHitErr[0][0] =
square(hit_localPhiErr * sinphi);
296 globalHitErr[0][1] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
297 globalHitErr[0][2] = 0;
298 globalHitErr[1][1] =
square(hit_localPhiErr * cosphi);;
299 globalHitErr[1][0] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
300 globalHitErr[1][2] = 0;
301 globalHitErr[2][2] =
square(hit_localZErr);
302 globalHitErr[2][0] = 0;
303 globalHitErr[2][1] = 0;
313 HepMatrix error_mea(2, 2, 0);
314 error =
sqrt(errors[ MY ][ MY ] +
322 sigma = distance / error;
335 int depDir,
int indDir)
339 Hep3Vector localHitPos;
340 HepMatrix localHitErr(3, 3, 0);
347 int noPoints = hitList.size();
350 HepMatrix A(noPoints, 2, 0);
353 HepVector y(noPoints, 0);
357 HepDiagMatrix V_y_inverse(noPoints, 0);
361 HepSymMatrix V_A, V_A_inverse;
363 int section = (*hitList.begin())->getSection();
364 int sector = (*hitList.begin())->getSector();
372 if (hit->getSection() != section || hit->getSector() != sector) {
380 CLHEP::Hep3Vector globalPos;
381 globalPos[0] = hit->getPositionX();
382 globalPos[1] = hit->getPositionY();
383 globalPos[2] = hit->getPositionZ();
391 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
392 double dn = nStrips - 1.5;
393 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
394 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
396 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
398 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
399 hit_localZErr = hit_localZErr *
sqrt(factor);
402 localHitErr[0][0] = 0.0;
403 localHitErr[0][1] = 0;
404 localHitErr[0][2] = 0;
405 localHitErr[1][1] = hit_localPhiErr;
406 localHitErr[1][0] = 0;
407 localHitErr[1][2] = 0;
408 localHitErr[2][2] = hit_localZErr;
409 localHitErr[2][0] = 0;
410 localHitErr[2][1] = 0;
415 indPos = localHitPos.x();
419 indPos = localHitPos.y();
423 indPos = localHitPos.z();
427 B2DEBUG(20,
"error in klm_trackSectorFit: illegal direction");
434 depPos = localHitPos.x();
438 depPos = localHitPos.y();
442 depPos = localHitPos.z();
446 B2DEBUG(20,
"error in klm_trackSectorFit: illegal direction");
452 A[ n ][ 1 ] = indPos;
456 if (localHitErr[ depDir ][ depDir ] > 0.0) {
457 V_y_inverse[ n ][ n ] = 1.0 / localHitErr[ depDir ][ depDir ];
459 V_y_inverse[ n ][ n ] = DBL_MAX;
464 V_A_inverse = V_y_inverse.similarityT(A);
467 V_A = V_A_inverse.inverse(ierr);
469 eta = V_A * A.T() * V_y_inverse * y;
473 HepMatrix residual = y - A * eta;
474 HepMatrix chisqr = residual.T() * V_y_inverse * residual;
476 return (chisqr.trace());
484 int depDir,
int indDir)
489 HepMatrix globalHitErr(3, 3, 0);
495 int noPoints = hitList.size();
498 HepMatrix A(noPoints, 2, 0);
501 HepVector y(noPoints, 0);
505 HepDiagMatrix V_y_inverse(noPoints, 0);
508 HepSymMatrix V_A, V_A_inverse;
518 corMod =
m_GeoPar->findModule(hit->getSection(), hit->getSector(), hit->getLayer());
520 CLHEP::Hep3Vector globalPos;
521 globalPos[0] = hit->getPositionX();
522 globalPos[1] = hit->getPositionY();
523 globalPos[2] = hit->getPositionZ();
531 int nStrips = hit->getPhiStripMax() - hit->getPhiStripMin() + 1;
532 double dn = nStrips - 1.5;
533 double factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.60;
534 hit_localPhiErr = hit_localPhiErr *
sqrt(factor);
536 nStrips = hit->getZStripMax() - hit->getZStripMin() + 1;
538 factor = std::pow((0.9 + 0.4 * dn * dn), 1.5) * 0.55;
539 hit_localZErr = hit_localZErr *
sqrt(factor);
543 double sinphi = globalOrigin[1] / globalOrigin.mag();
544 double cosphi = globalOrigin[0] / globalOrigin.mag();
546 globalHitErr[0][0] =
square(hit_localPhiErr * sinphi);
547 globalHitErr[0][1] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
548 globalHitErr[0][2] = 0;
549 globalHitErr[1][1] =
square(hit_localPhiErr * cosphi);;
550 globalHitErr[1][0] = (hit_localPhiErr * sinphi) * (hit_localPhiErr * cosphi);
551 globalHitErr[1][2] = 0;
552 globalHitErr[2][2] =
square(hit_localZErr);;
553 globalHitErr[2][0] = 0;
554 globalHitErr[2][1] = 0;
559 indPos = globalPos.x();
563 indPos = globalPos.y();
567 indPos = globalPos.z();
571 B2DEBUG(20,
"error in bklm trackFit: illegal direction");
578 depPos = globalPos.x();
582 depPos = globalPos.y();
586 depPos = globalPos.z();
590 B2DEBUG(20,
"error in bklm trackFit: illegal direction");
596 A[ n ][ 1 ] = indPos;
600 double error_raw = globalHitErr[indDir][indDir] + globalHitErr[depDir][depDir];
603 double weight = error_raw;
605 V_y_inverse[ n ][ n ] = 1.0 / weight;
607 V_y_inverse[ n ][ n ] = DBL_MAX;
615 V_A_inverse = V_y_inverse.similarityT(A);
618 V_A = V_A_inverse.inverse(ierr);
620 eta = V_A * A.T() * V_y_inverse * y;
624 HepMatrix residual = y - A * eta;
625 HepMatrix chisqr = residual.T() * V_y_inverse * residual;
627 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.
bool inRPC() const
Determine whether this 2D hit is in RPC or scintillator.
int getLayer() const
Get layer number.
int getZStripMax() const
Get last strip number for z plane.
int getSection() const
Get section number.
float getPositionZ() const
Get hit global position z coordinate.
int getSector() const
Get sector number.
float getPositionX() const
Get hit global position x coordinate.
int getPhiStripMin() const
Get strip number for phi plane.
int getZStripMin() const
Get strip number for z plane.
int getPhiStripMax() const
Get last strip number for phi plane.
float getPositionY() const
Get hit global position y coordinate.
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.
constexpr T square(const T &x)
Calculate the square of the input.
double sqrt(double a)
sqrt for double
Abstract base class for different kinds of events.