Belle II Software development
FacetFitter.cc
1/**************************************************************************
2 * basf2 (Belle II Analysis Software Framework) *
3 * Author: The Belle II Collaboration *
4 * *
5 * See git log for contributors and copyright holders. *
6 * This file is licensed under LGPL-3.0, see LICENSE.md. *
7 **************************************************************************/
8#include <tracking/trackFindingCDC/fitting/FacetFitter.h>
9
10#include <tracking/trackFindingCDC/eventdata/hits/CDCFacet.h>
11#include <tracking/trackFindingCDC/eventdata/hits/CDCWireHit.h>
12#include <tracking/trackFindingCDC/geometry/UncertainParameterLine2D.h>
13#include <tracking/trackFindingCDC/geometry/ParameterLine2D.h>
14#include <tracking/trackFindingCDC/geometry/Vector2D.h>
15
16#include <tracking/trackFindingCDC/numerics/EigenView.h>
17
18#include <Eigen/Core>
19
20#include <Math/Functor.h>
21#include <Math/BrentMinimizer1D.h>
22
23using namespace Belle2;
24using namespace TrackFindingCDC;
25
26namespace {
27
28 template<int N>
29 Vector2D getCenterForwardDirection(const Matrix<double, N, 3>& xyl)
30 {
32 Vector2D coordinate(xyl(N - 1, 0) - xyl(0, 0), xyl(N - 1, 1) - xyl(0, 1));
33 return coordinate.unit();
34 }
35
36 template<int N>
37 Vector2D getTangentialForwardDirection(const Matrix<double, N, 3>& xyl)
38 {
40 Vector2D fromPos(xyl(0, 0), xyl(0, 1));
41 double fromL = xyl(0, 2);
42
43 Vector2D toPos(xyl(N - 1, 0), xyl(N - 1, 1));
44 double toL = xyl(N - 1, 2);
45
46 ParameterLine2D tangentLine = ParameterLine2D::touchingCircles(fromPos, fromL, toPos, toL);
47 Vector2D coordinate = tangentLine.tangential();
48 return coordinate.unit();
49 }
50
51 template<int N>
52 void rotate(Vector2D coordinate, Matrix<double, N, 3>& xyl)
53 {
54 Matrix<double, 3, 3> rot = Matrix<double, 3, 3>::Identity();
55 rot(0, 0) = coordinate.x();
56 rot(0, 1) = -coordinate.y();
57 rot(1, 0) = coordinate.y();
58 rot(1, 1) = coordinate.x();
59 rot(2, 2) = 1; // Drift length remains the same.
60 xyl = xyl * rot;
61 }
62
63 void unrotate(Vector2D coordinate, Vector2D& vec)
64 {
65 // Inverse rotation is accomblished by taking the angle to the opposite
66 // which is equivalent to flipping the second coordinate.
67 vec = vec.passiveRotatedBy(coordinate.flippedSecond());
68 }
69
70 Eigen::Vector2d fitPhiVecZeroSteps(const Eigen::Matrix<double, 3, 3>& xylCov, double& chi2)
71 {
72 chi2 = xylCov(1, 1) + 2 * xylCov(1, 2) + xylCov(2, 2);
73 return Eigen::Vector2d(1, 0);
74 }
75
76 Eigen::Vector2d fitPhiVecOneStep(const Eigen::Matrix<double, 3, 3>& xylCov, double& chi2)
77 {
78 const double phi = (xylCov(0, 1) + xylCov(0, 2)) / xylCov(0, 0);
79 chi2 = xylCov(1, 1) + 2 * xylCov(1, 2) + xylCov(2, 2) - phi * (xylCov(0, 1) + xylCov(0, 2));
80 return Eigen::Vector2d(std::cos(phi), std::sin(phi));
81 }
82
83 Eigen::Vector2d fitPhiVecBrent(const Eigen::Matrix<double, 3, 3>& xylCov, int nIter, double& chi2)
84 {
85 const Eigen::Matrix< double, 2, 2> A = xylCov.topLeftCorner<2, 2>();
86 const Eigen::Matrix< double, 2, 1> b = xylCov.topRightCorner<2, 1>();
87 const double c = xylCov(2, 2);
88
89 auto calcReducedChi2 = [&A, &b](double phi) -> double {
90 Eigen::Matrix<double, 2, 1> normal(std::sin(phi), -std::cos(phi));
91 return ((normal.transpose() * A - 2 * b.transpose()) * normal)[0];
92 };
93
94 ROOT::Math::Functor1D functor(calcReducedChi2);
95 ROOT::Math::BrentMinimizer1D bm;
96 bm.SetFunction(functor, -M_PI / 2, M_PI / 2);
97 bm.Minimize(nIter); // #iterations, abs. error, rel. error
98
99 chi2 = bm.FValMinimum() + c;
100 const double phi = bm.XMinimum();
101 return Eigen::Vector2d(std::cos(phi), std::sin(phi));
102 }
103
104 template<int N>
105 UncertainParameterLine2D fit(Matrix<double, N, 3> xylIn,
106 Matrix<double, N, 1> wIn,
107 int nSteps)
108 {
110 Vector2D coordinate = getTangentialForwardDirection(xylIn);
111 // Sometimes the calculation of the tangent fails due to misestimated dirft lengths
112 // Make best effort the continue the calculation
113 if (coordinate.hasNAN()) {
114 coordinate = getCenterForwardDirection(xylIn);
115 }
116
117 rotate(coordinate, xylIn);
118
119 auto xyl = mapToEigen(xylIn);
120 auto w = mapToEigen(wIn).array();
121
122 Eigen::Array< double, 1, 3> averages = (xyl.array().colwise() * w).colwise().sum() / w.sum();
123 Eigen::Matrix< double, N, 3> deltas = xyl.array().rowwise() - averages;
124 Eigen::Matrix< double, N, 3> weightedDeltas = deltas.array().colwise() * w;
125 Eigen::Matrix< double, 3, 3> covariances = deltas.transpose() * weightedDeltas / w.sum();
126
127 Eigen::Vector2d phiVec;
128 double chi2 = 0.0;
129 if (nSteps == 0) {
130 phiVec = fitPhiVecZeroSteps(covariances, chi2);
131 } else if (nSteps == 1) {
132 phiVec = fitPhiVecOneStep(covariances, chi2);
133 } else {
134 phiVec = fitPhiVecBrent(covariances, nSteps, chi2);
135 }
136 chi2 *= w.sum();
137
138 double meanArcLength = averages.topLeftCorner<1, 2>().matrix() * phiVec;
139 double varArcLength = phiVec.transpose() * covariances.topLeftCorner<2, 2>() * phiVec;
140 double p = w.sum();
141
142 using namespace NLineParameterIndices;
143 LinePrecision linePrecision;
144 linePrecision(c_Phi0, c_Phi0) = p * (varArcLength + meanArcLength * meanArcLength);
145 linePrecision(c_Phi0, c_I) = p * meanArcLength;
146 linePrecision(c_I, c_Phi0) = p * meanArcLength;
147 linePrecision(c_I, c_I) = p;
148 LineCovariance lineCovariance = LineUtil::covarianceFromFullPrecision(linePrecision);
149
150 Vector2D tangential(phiVec(0), phiVec(1));
151 Vector2D n12 = tangential.orthogonal(ERotation::c_Clockwise);
152 double n0 = averages(2) - averages(0) * n12.x() - averages(1) * n12.y();
153 Vector2D support = -n12 * n0;
154
155 // Transform the normal vector back into the original coordinate system.
156 unrotate(coordinate, support);
157 unrotate(coordinate, tangential);
158
159 ParameterLine2D parameterLine2D(support, tangential);
160 int ndf = N - 2;
161 return UncertainParameterLine2D(parameterLine2D, lineCovariance, chi2, ndf);
162 }
163
164}
165
166double FacetFitter::fit(const CDCFacet& facet, int nSteps)
167{
168 // Measurement matrix
169 Matrix<double, 3, 3> xyl = Matrix<double, 3, 3>::Zero();
170
171 // Weight matrix
172 Matrix<double, 3, 1> w = Matrix<double, 3, 1>::Zero();
173
174 const CDCRLWireHit& startRLWireHit = facet.getStartRLWireHit();
175 const CDCRLWireHit& middleRLWireHit = facet.getMiddleRLWireHit();
176 const CDCRLWireHit& endRLWireHit = facet.getEndRLWireHit();
177
178 const Vector2D support = middleRLWireHit.getWireHit().getRefPos2D();
179
180 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
181 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
182 xyl(0, 0) = startWirePos2D.x() - support.x();
183 xyl(0, 1) = startWirePos2D.y() - support.y();
184 xyl(0, 2) = startRLWireHit.getSignedRefDriftLength();
185 w(0) = 1.0 / startDriftLengthVar;
186
187 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
188 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
189 xyl(1, 0) = middleWirePos2D.x() - support.x();
190 xyl(1, 1) = middleWirePos2D.y() - support.y();
191 xyl(1, 2) = middleRLWireHit.getSignedRefDriftLength();
192 w(1) = 1.0 / middleDriftLengthVar;
193
194 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
195 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
196 xyl(2, 0) = endWirePos2D.x() - support.x();
197 xyl(2, 1) = endWirePos2D.y() - support.y();
198 xyl(2, 2) = endRLWireHit.getSignedRefDriftLength();
199 w(2) = 1.0 / endDriftLengthVar;
200
201 UncertainParameterLine2D fitLine{ ::fit(std::move(xyl), std::move(w), nSteps) };
202 fitLine.passiveMoveBy(-support);
203 facet.setFitLine(fitLine);
204 return fitLine.chi2();
205}
206
207
209 const CDCFacet& toFacet,
210 int nSteps)
211{
212 // Observations matrix
213 Matrix<double, 6, 3> xyl = Matrix<double, 6, 3>::Zero();
214
215 // Weight matrix
216 Matrix<double, 6, 1> w = Matrix<double, 6, 1>::Zero();
217
218 const Vector2D support = Vector2D::average(fromFacet.getMiddleWireHit().getRefPos2D(),
219 toFacet.getMiddleWireHit().getRefPos2D());
220 {
221 const CDCRLWireHit& startRLWireHit = fromFacet.getStartRLWireHit();
222 const CDCRLWireHit& middleRLWireHit = fromFacet.getMiddleRLWireHit();
223 const CDCRLWireHit& endRLWireHit = fromFacet.getEndRLWireHit();
224
225 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
226 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
227 xyl(0, 0) = startWirePos2D.x() - support.x();
228 xyl(0, 1) = startWirePos2D.y() - support.y();
229 xyl(0, 2) = startRLWireHit.getSignedRefDriftLength();
230 w(0) = 1.0 / startDriftLengthVar;
231
232 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
233 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
234 xyl(1, 0) = middleWirePos2D.x() - support.x();
235 xyl(1, 1) = middleWirePos2D.y() - support.y();
236 xyl(1, 2) = middleRLWireHit.getSignedRefDriftLength();
237 w(1) = 1.0 / middleDriftLengthVar;
238
239 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
240 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
241 xyl(2, 0) = endWirePos2D.x() - support.x();
242 xyl(2, 1) = endWirePos2D.y() - support.y();
243 xyl(2, 2) = endRLWireHit.getSignedRefDriftLength();
244 w(2) = 1.0 / endDriftLengthVar;
245 }
246
247 {
248 const CDCRLWireHit& startRLWireHit = toFacet.getStartRLWireHit();
249 const CDCRLWireHit& middleRLWireHit = toFacet.getMiddleRLWireHit();
250 const CDCRLWireHit& endRLWireHit = toFacet.getEndRLWireHit();
251
252 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
253 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
254 xyl(3, 0) = startWirePos2D.x() - support.x();
255 xyl(3, 1) = startWirePos2D.y() - support.y();
256 xyl(3, 2) = startRLWireHit.getSignedRefDriftLength();
257 w(3) = 1.0 / startDriftLengthVar;
258
259 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
260 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
261 xyl(4, 0) = middleWirePos2D.x() - support.x();
262 xyl(4, 1) = middleWirePos2D.y() - support.y();
263 xyl(4, 2) = middleRLWireHit.getSignedRefDriftLength();
264 w(4) = 1.0 / middleDriftLengthVar;
265
266 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
267 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
268 xyl(5, 0) = endWirePos2D.x() - support.x();
269 xyl(5, 1) = endWirePos2D.y() - support.y();
270 xyl(5, 2) = endRLWireHit.getSignedRefDriftLength();
271 w(5) = 1.0 / endDriftLengthVar;
272 }
273
274 UncertainParameterLine2D fitLine{ ::fit(std::move(xyl), std::move(w), nSteps) };
275 fitLine.passiveMoveBy(-support);
276 return fitLine;
277}
278
279
281 Matrix<double, 3, 1> w,
282 int nSteps)
283{
284 return ::fit(std::move(xyl), std::move(w), nSteps);
285}
Class representing a triple of neighboring oriented wire with additional trajectory information.
Definition CDCFacet.h:32
void setFitLine(const UncertainParameterLine2D &fitLine) const
Setter for the contained line fit information.
Definition CDCFacet.h:67
const CDCWireHit & getMiddleWireHit() const
Getter for the hit wire of the second oriented wire hit.
CDCRLWireHit & getStartRLWireHit()
Getter for the first oriented wire hit.
CDCRLWireHit & getEndRLWireHit()
Getter for the third oriented wire hit.
CDCRLWireHit & getMiddleRLWireHit()
Getter for the second oriented wire hit.
Class representing an oriented hit wire including a hypotheses whether the causing track passes left ...
const CDCWireHit & getWireHit() const
Getter for the wire hit associated with the oriented hit.
double getRefDriftLengthVariance() const
Getter for the variance of the drift length at the reference position of the wire.
double getSignedRefDriftLength() const
Getter for the drift length at the reference position of the wire.
const Vector2D & getRefPos2D() const
The two dimensional reference position (z=0) of the underlying wire.
static double fit(const CDCFacet &facet, int nSteps=100)
Fits a proper line to facet and returns the chi2.
A line with a support point and tangential vector.
static ParameterLine2D touchingCircles(const Vector2D &fromCenter, double fromSignedRadius, const Vector2D &toCenter, double toSignedRadius)
Constructs a line touching two circles in one point each.
const Vector2D & tangential() const
Gives the tangential vector of the line.
static PlainMatrix< T, M, N > Identity()
Construct an identity matrix.
Definition PlainMatrix.h:74
static PlainMatrix< T, M, N > Zero()
Construct a matrix initialized with zeros.
Definition PlainMatrix.h:67
A parameter line including including an line covariance matrix which is interpreted as located in the...
void passiveMoveBy(const Vector2D &by)
Moves the coordinate system by the vector by.
A two dimensional vector which is equipped with functions for correct handling of orientation relate...
Definition Vector2D.h:32
Vector2D flippedSecond() const
Makes a copy of the vector with the second coordinate flipped (no difference between active and passi...
Definition Vector2D.h:365
static Vector2D average(const Vector2D &one, const Vector2D &two)
Constructs the average of two vectors.
Definition Vector2D.h:84
double x() const
Getter for the x coordinate.
Definition Vector2D.h:595
Vector2D orthogonal() const
Orthogonal vector to the counterclockwise direction.
Definition Vector2D.h:289
bool hasNAN() const
Checks if one of the coordinates is NAN.
Definition Vector2D.h:149
double y() const
Getter for the y coordinate.
Definition Vector2D.h:605
Vector2D unit() const
Returns a unit vector colaligned with this.
Definition Vector2D.h:321
Namespace to hide the contained enum constants.
Abstract base class for different kinds of events.