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();
370 for (
const KLMHit2d* hit : hitList) {
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;
514 for (
const KLMHit2d* hit : hitList) {
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 distanceToHit(const KLMHit2d *hit, double &error, double &sigma)
Distance from track to a hit in the plane of the module.
int m_NumHit
the number of hits on this track
BKLMTrackFitter()
Default constructor.
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.
double globalDistanceToHit(const KLMHit2d *hit, double &error, double &sigma)
Distance from track to a hit in the global system.
double fit1dTrack(const std::list< KLMHit2d * > &hitList, CLHEP::HepVector &eta, CLHEP::HepSymMatrix &error, int depDir, int indDir)
do fit in the global system
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.