11 #include <tracking/trackFindingVXD/trackQualityEstimators/QualityEstimatorCircleFit.h>
12 #include <tracking/trackFindingVXD/utilities/CalcCurvatureSignum.h>
14 #include <framework/logging/Logger.h>
23 if (measurements.size() < 4) {
24 if (measurements.size() == 3)
return 0.2;
33 double stopper = 0.000000001;
34 double meanX = 0, meanY = 0, meanX2 = 0, meanY2 = 0, meanR2 = 0, meanR4 = 0, meanXR2 = 0, meanYR2 = 0, meanXY = 0;
35 double sumWeights = 0, divisor;
36 double tuningParameter = 1.;
40 double weight = 1. / (sqrt(hit->getPositionError().X() * hit->getPositionError().X() + hit->getPositionError().Y() *
41 hit->getPositionError().Y()) * tuningParameter);
43 if (std::isnan(weight) or std::isinf(weight)) {
44 B2ERROR(
"QualityEstimators::circleFit, chosen sigma is 'nan': " << weight <<
", setting arbitrary error: "
45 << stopper <<
")"); weight = stopper;
47 double x = hit->getPosition().X();
48 double y = hit->getPosition().Y();
54 meanXY += x * y * weight;
55 meanX2 += x2 * weight;
56 meanY2 += y2 * weight;
57 meanXR2 += x * r2 * weight;
58 meanYR2 += y * r2 * weight;
59 meanR2 += r2 * weight;
60 meanR4 += r2 * r2 * weight;
62 divisor = 1. / sumWeights;
74 double covXX = meanX2 - meanX * meanX;
75 double covXY = meanXY - meanX * meanY;
76 double covYY = meanY2 - meanY * meanY;
77 double covXR2 = meanXR2 - meanX * meanR2;
78 double covYR2 = meanYR2 - meanY * meanR2;
79 double covR2R2 = meanR4 - meanR2 * meanR2;
86 double q1 = covR2R2 * covXY - covXR2 * covYR2;
87 double q2 = covR2R2 * (covXX - covYY) - covXR2 * covXR2 + covYR2 * covYR2;
90 double pocaPhi = 0.5 * atan2(2. * q1, q2);
92 double sinPhi = sin(pocaPhi);
93 double cosPhi = cos(pocaPhi);
94 double kappa = (sinPhi * covXR2 - cosPhi * covYR2) / covR2R2;
95 double delta = -kappa * meanR2 + sinPhi * meanX - cosPhi * meanY;
96 double rootTerm = sqrt(1. - 4.*delta * kappa);
98 double curvature = 2.*kappa / (rootTerm);
99 double pocaD = 2.*delta / (1. + rootTerm);
102 if ((curvature < 0 && clockwise) || (curvature > 0 && !clockwise)) {
104 curvature = -curvature;
108 double radius = 1. / curvature;
109 double absRadius = fabs(radius);
113 double chi2 = sumWeights * (1. + curvature * pocaD) * (1. + curvature * pocaD)
114 * (sinPhi * sinPhi * covXX - 2.*sinPhi * cosPhi * covXY + cosPhi * cosPhi * covYY - kappa * kappa * covR2R2);
118 return TMath::Prob(chi2, measurements.size() - 3);