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