Belle II Software  release-05-01-25
ExtendedRiemannsMethod.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * Copyright(C) 2014 - 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/ExtendedRiemannsMethod.h>
11 
12 #include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
13 #include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
14 
15 #include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectory2D.h>
16 
17 #include <tracking/trackFindingCDC/geometry/UncertainPerigeeCircle.h>
18 #include <tracking/trackFindingCDC/geometry/PerigeeParameters.h>
19 #include <tracking/trackFindingCDC/geometry/Vector2D.h>
20 
21 #include <tracking/trackFindingCDC/numerics/EigenView.h>
22 
23 #include <framework/logging/Logger.h>
24 
25 #include <Eigen/Eigen>
26 #include <Eigen/Core>
27 
28 using namespace Belle2::TrackFindingCDC;
29 
31  : m_lineConstrained(false)
32  , m_originConstrained(false)
33 {
34 }
35 
37  CDCObservations2D& observations2D) const
38 {
39  size_t nObservations = observations2D.size();
40  trajectory2D.clear();
41  if (not nObservations) return;
42 
43  Vector2D origin = Vector2D(0.0, 0.0);
44  Vector2D centralPoint = observations2D.getCentralPoint();
45 
46  const Vector2D& ref = isOriginConstrained() ? origin : centralPoint;
47  observations2D.passiveMoveBy(ref);
48 
49  UncertainPerigeeCircle perigeeCircle = fitInternal(observations2D);
50 
51  double frontX = observations2D.getX(0);
52  double frontY = observations2D.getY(0);
53  Vector2D frontPos(frontX, frontY);
54 
55  double backX = observations2D.getX(nObservations - 1);
56  double backY = observations2D.getY(nObservations - 1);
57  Vector2D backPos(backX, backY);
58 
59  Vector2D overPos(0, 0);
60  double totalPerps = (perigeeCircle->arcLengthBetween(frontPos, overPos) +
61  perigeeCircle->arcLengthBetween(overPos, backPos));
62 
63  if (totalPerps < 0) {
64  perigeeCircle.reverse();
65  }
66 
67  trajectory2D = CDCTrajectory2D(ref, perigeeCircle);
68 }
69 
70 namespace {
71 
75  enum EParabolicIndices {
76  iW = 0,
77  iX = 1,
78  iY = 2,
79  iR2 = 3,
80  iL = 4
81  };
82  }
84  PerigeeCircle fit(const Eigen::Matrix< double, 5, 5 >& sumMatrix,
85  bool lineConstrained = false,
86  bool originConstrained = false)
87  {
88  using namespace NParabolicParameterIndices;
89  // Solve the normal equation X * n = y
90  if (lineConstrained) {
91  if (originConstrained) {
92  Eigen::Matrix< double, 2, 2> X = sumMatrix.block<2, 2>(1, 1);
93  Eigen::Matrix< double, 2, 1> y = sumMatrix.block<2, 1>(1, iL);
94  Eigen::Matrix< double, 2, 1> n = X.ldlt().solve(y);
95  return PerigeeCircle::fromN(0.0, n(0), n(1), 0.0);
96 
97  } else {
98  Eigen::Matrix< double, 3, 3> X = sumMatrix.block<3, 3>(0, 0);
99  Eigen::Matrix< double, 3, 1> y = sumMatrix.block<3, 1>(0, iL);
100  Eigen::Matrix< double, 3, 1> n = X.ldlt().solve(y);
101  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), 0.0);
102 
103  }
104 
105  } else {
106  if (originConstrained) {
107  Eigen::Matrix< double, 3, 3> X = sumMatrix.block<3, 3>(1, 1);
108  Eigen::Matrix< double, 3, 1> y = sumMatrix.block<3, 1>(1, iL);
109  Eigen::Matrix< double, 3, 1> n = X.ldlt().solve(y);
110  return PerigeeCircle::fromN(0.0, n(0), n(1), n(2));
111 
112  } else {
113  Eigen::Matrix< double, 4, 4> X = sumMatrix.block<4, 4>(0, 0);
114  Eigen::Matrix< double, 4, 1> y = sumMatrix.block<4, 1>(0, iL);
115  Eigen::Matrix< double, 4, 1> n = X.ldlt().solve(y);
116  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), n(iR2));
117 
118  }
119  }
120  }
121 
122 
124  PerigeeCircle fit(Eigen::Matrix< double, 4, 4 > sumMatrix,
125  bool lineConstrained = false,
126  bool originConstrained = false)
127  {
128  // Solve the normal equation min_n n^T * X * n
129  // n is the smallest eigenvector
130 
131  using namespace NParabolicParameterIndices;
132  if (lineConstrained) {
133  if (originConstrained) {
134  Eigen::Matrix< double, 2, 2> X = sumMatrix.block<2, 2>(1, 1);
135  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(X);
136  Eigen::Matrix<double, 2, 1> n = eigensolver.eigenvectors().col(0);
137  if (eigensolver.info() != Eigen::Success) {
138  B2WARNING("SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
139  }
140  return PerigeeCircle::fromN(0.0, n(0), n(1), 0.0);
141 
142  } else {
143  Eigen::Matrix< double, 3, 3> X = sumMatrix.block<3, 3>(0, 0);
144  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(X);
145  Eigen::Matrix<double, 3, 1> n = eigensolver.eigenvectors().col(0);
146  if (eigensolver.info() != Eigen::Success) {
147  B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
148  }
149  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), 0.0);
150 
151  }
152  } else {
153  if (originConstrained) {
154  Eigen::Matrix< double, 3, 3> X = sumMatrix.block<3, 3>(1, 1);
155  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(X);
156  Eigen::Matrix<double, 3, 1> n = eigensolver.eigenvectors().col(0);
157  if (eigensolver.info() != Eigen::Success) {
158  B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
159  }
160  return PerigeeCircle::fromN(0.0, n(0), n(1), n(2));
161 
162  } else {
163  Eigen::Matrix< double, 4, 4 > X = sumMatrix.block<4, 4>(0, 0);
164  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 4, 4> > eigensolver(X);
165  Eigen::Matrix<double, 4, 1> n = eigensolver.eigenvectors().col(0);
166  if (eigensolver.info() != Eigen::Success) {
167  B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
168  }
169 
170  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), n(iR2));
171  }
172  }
173  }
174 
176  PerigeeCircle fitSeperateOffset(Eigen::Matrix< double, 4, 1 > means,
177  Eigen::Matrix< double, 4, 4 > c,
178  bool lineConstrained = false)
179  {
180  // Solve the normal equation min_n n^T * c * n
181  // for the plane normal and move the plain by the offset
182  // n is the smallest eigenvector
183  using namespace NParabolicParameterIndices;
184  if (lineConstrained) {
185  Eigen::Matrix< double, 2, 2> X = c.block<2, 2>(1, 1);
186  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(X);
187  if (eigensolver.info() != Eigen::Success) {
188  B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
189  }
190 
191  //the eigenvalues are generated in increasing order
192  //we are interested in the lowest one since we want to compute the normal vector of the plane
193  Eigen::Matrix<double, 4, 1> n;
194  n.middleRows<2>(iX) = eigensolver.eigenvectors().col(0);
195  n(iW) = -means.middleRows<2>(iX).transpose() * n.middleRows<2>(iX);
196  n(iR2) = 0.;
197  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), n(iR2));
198 
199  } else {
200  Eigen::Matrix< double, 3, 3> X = c.block<3, 3>(1, 1);
201  Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(X);
202  if (eigensolver.info() != Eigen::Success) {
203  B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
204  }
205 
206  // the eigenvalues are generated in increasing order
207  // we are interested in the lowest one since we want to compute the normal vector of the plane
208  Eigen::Matrix<double, 4, 1> n;
209  n.middleRows<3>(iX) = eigensolver.eigenvectors().col(0);
210  n(iW) = -means.middleRows<3>(iX).transpose() * n.middleRows<3>(iX);
211  return PerigeeCircle::fromN(n(iW), n(iX), n(iY), n(iR2));
212 
213  }
214  }
215 
216 
218  double calcChi2(const PerigeeCircle& parameters,
219  const Eigen::Matrix< double, 5, 5 >& s)
220  {
221  Eigen::Matrix<double, 5, 1> n;
222  n(0) = parameters.n0();
223  n(1) = parameters.n1();
224  n(2) = parameters.n2();
225  n(3) = parameters.n3();
226  n(4) = -1;
227 
228  double chi2 = n.transpose() * s * n;
229  return chi2;
230  }
231 
232 
234  double calcChi2(const PerigeeCircle& parameters,
235  const Eigen::Matrix< double, 4, 4 >& s)
236  {
237  Eigen::Matrix<double, 4, 1> n;
238  n(0) = parameters.n0();
239  n(1) = parameters.n1();
240  n(2) = parameters.n2();
241  n(3) = parameters.n3();
242 
243  double chi2 = n.transpose() * s * n;
244  return chi2;
245  }
246 
247 
248  PerigeePrecision calcPrecision(const PerigeeCircle& parameters,
249  const Eigen::Matrix< double, 4, 4 >& s,
250  bool lineConstrained = false,
251  bool originConstrained = false)
252  {
253  const double impact = parameters.impact();
254  const Vector2D& phi0Vec = parameters.tangential();
255  const double curvature = parameters.curvature();
256 
257  using namespace NPerigeeParameterIndices;
258  Eigen::Matrix<double, 4, 3> ambiguity = Eigen::Matrix<double, 4, 3> ::Zero();
259 
260  ambiguity(0, c_Curv) = impact * impact / 2;
261  ambiguity(1, c_Curv) = phi0Vec.y() * impact;
262  ambiguity(2, c_Curv) = -phi0Vec.x() * impact;
263  ambiguity(3, c_Curv) = 1.0 / 2.0;
264 
265  ambiguity(0, c_Phi0) = 0;
266  ambiguity(1, c_Phi0) = phi0Vec.x() * (1 + curvature * impact);
267  ambiguity(2, c_Phi0) = phi0Vec.y() * (1 + curvature * impact);
268  ambiguity(3, c_Phi0) = 0;
269 
270  ambiguity(0, c_I) = 1 + curvature * impact;
271  ambiguity(1, c_I) = phi0Vec.y() * curvature;
272  ambiguity(2, c_I) = -phi0Vec.x() * curvature;
273  ambiguity(3, c_I) = 0;
274 
275  Eigen::Matrix<double, 3, 3> perigeePrecision = ambiguity.transpose() * s * ambiguity;
276  // Zero out the unfitted parameters from the precision matrix
277  if (lineConstrained) {
278  perigeePrecision.row(c_Curv) = Eigen::Matrix<double, 1, 3>::Zero();
279  perigeePrecision.col(c_Curv) = Eigen::Matrix<double, 3, 1>::Zero();
280  }
281 
282  if (originConstrained) {
283  perigeePrecision.row(c_I) = Eigen::Matrix<double, 1, 3>::Zero();
284  perigeePrecision.col(c_I) = Eigen::Matrix<double, 3, 1>::Zero();
285  }
286 
287  PerigeePrecision result;
288  mapToEigen(result) = perigeePrecision;
289  return result;
290  }
291 
292 }
293 
294 
295 
297 {
298  using namespace NParabolicParameterIndices;
299 
300  // Matrix of weighted sums
301  Eigen::Matrix< double, 5, 5 > s = getWXYRLSumMatrix(observations2D);
302 
303  // The same as above without drift lengths
304  Eigen::Matrix<double, 4, 4> sNoL = s.block<4, 4>(0, 0);
305 
306  // Determine NDF : Circle fit eats up to 3 degrees of freedom depending on the constraints
307  size_t ndf = observations2D.size() - 1;
308 
309  if (not isOriginConstrained()) {
310  --ndf;
311  }
312 
313  if (not isLineConstrained()) {
314  --ndf;
315  }
316 
317  // Parameters to be fitted
318  UncertainPerigeeCircle resultCircle;
319  double chi2 = 0;
320 
321  size_t nObservationsWithDriftRadius = observations2D.getNObservationsWithDriftRadius();
322  if (nObservationsWithDriftRadius > 0) {
323  resultCircle = UncertainPerigeeCircle(::fit(s, isLineConstrained(), isOriginConstrained()));
324  chi2 = calcChi2(resultCircle, s);
325  } else {
326  if (not isOriginConstrained()) {
327  // Alternative implementation for comparision
328 
329  // Matrix of averages
330  Eigen::Matrix< double, 4, 4> aNoL = sNoL / sNoL(iW);
331 
332  // Measurement means
333  Eigen::Matrix< double, 4, 1> meansNoL = aNoL.row(iW);
334 
335  // Covariance matrix
336  Eigen::Matrix< double, 4, 4> cNoL = aNoL - meansNoL * meansNoL.transpose();
337 
338  resultCircle = UncertainPerigeeCircle(fitSeperateOffset(meansNoL, cNoL, isLineConstrained()));
339 
340  } else {
341  resultCircle = UncertainPerigeeCircle(::fit(sNoL, isLineConstrained(), isOriginConstrained()));
342  }
343 
344  chi2 = calcChi2(resultCircle, sNoL);
345  }
346 
347  // Covariance calculation does not need the drift lengths, which is why we do not forward them.
348  PerigeePrecision perigeePrecision =
349  calcPrecision(resultCircle, sNoL, isLineConstrained(), isOriginConstrained());
350 
351  // Use in pivoting in case the matrix is not full rank as it is for the constrained cases
352  PerigeeCovariance perigeeCovariance = PerigeeUtil::covarianceFromPrecision(perigeePrecision);
353 
354  resultCircle.setNDF(ndf);
355  resultCircle.setChi2(chi2);
356  resultCircle.setPerigeeCovariance(perigeeCovariance);
357  return resultCircle;
358 }
Belle2::TrackFindingCDC::CDCObservations2D::size
std::size_t size() const
Returns the number of observations stored.
Definition: CDCObservations2D.h:88
Belle2::TrackFindingCDC::ExtendedRiemannsMethod::isLineConstrained
bool isLineConstrained() const
Getter for the indictor that lines should be fitted by this fitter.
Definition: ExtendedRiemannsMethod.h:49
Belle2::TrackFindingCDC::UncertainPerigeeCircle::setChi2
void setChi2(const double chi2)
Setter for the chi square value of the circle fit.
Definition: UncertainPerigeeCircle.h:190
Belle2::TrackFindingCDC::CDCObservations2D::getX
double getX(int iObservation) const
Getter for the x value of the observation at the given index.
Definition: CDCObservations2D.h:118
Belle2::TrackFindingCDC::Vector2D
A two dimensional vector which is equipped with functions for correct handeling of orientation relat...
Definition: Vector2D.h:37
Belle2::TrackFindingCDC::ExtendedRiemannsMethod::fitInternal
UncertainPerigeeCircle fitInternal(CDCObservations2D &observations2D) const
Internal method doing the heavy work.
Definition: ExtendedRiemannsMethod.cc:296
Belle2::TrackFindingCDC::PerigeeCircle::fromN
static PerigeeCircle fromN(double n0, double n1, double n2, double n3=0)
Constructor with the four parameters of the generalized circle.
Belle2::TrackFindingCDC::Vector2D::y
double y() const
Getter for the y coordinate.
Definition: Vector2D.h:619
Belle2::TrackFindingCDC::UncertainPerigeeCircle::setPerigeeCovariance
void setPerigeeCovariance(const PerigeeCovariance &perigeeCovariance)
Setter for the whole covariance matrix of the perigee parameters.
Definition: UncertainPerigeeCircle.h:160
Belle2::TrackFindingCDC::CDCObservations2D::passiveMoveBy
void passiveMoveBy(const Vector2D &origin)
Moves all observations passively such that the given vector becomes to origin of the new coordinate s...
Definition: CDCObservations2D.cc:347
Belle2::TrackFindingCDC::PerigeeCircle::arcLengthBetween
double arcLengthBetween(const Vector2D &from, const Vector2D &to) const
Calculates the arc length between two points of closest approach on the circle.
Definition: PerigeeCircle.cc:246
Belle2::TrackFindingCDC::ExtendedRiemannsMethod::ExtendedRiemannsMethod
ExtendedRiemannsMethod()
Constructor setting the default constraints.
Definition: ExtendedRiemannsMethod.cc:30
Belle2::TrackFindingCDC::ExtendedRiemannsMethod::isOriginConstrained
bool isOriginConstrained() const
Getter for the indictor that curves through the origin should be fitted by this fitter.
Definition: ExtendedRiemannsMethod.h:55
Belle2::TrackFindingCDC::NPerigeeParameterIndices::c_I
@ c_I
Constant to address the impact parameter.
Definition: PerigeeParameters.h:42
Belle2::TrackFindingCDC::CDCTrajectory2D
Particle trajectory as it is seen in xy projection represented as a circle.
Definition: CDCTrajectory2D.h:46
Belle2::TrackFindingCDC::ExtendedRiemannsMethod::update
void update(CDCTrajectory2D &trajectory2D, CDCObservations2D &observations2D) const
Executes the fit and updates the trajectory parameters This may render the information in the observa...
Definition: ExtendedRiemannsMethod.cc:36
Belle2::TrackFindingCDC::CDCObservations2D::getY
double getY(int iObservation) const
Getter for the y value of the observation at the given index.
Definition: CDCObservations2D.h:124
Belle2::TrackFindingCDC::NPerigeeParameterIndices::c_Phi0
@ c_Phi0
Constant to address the azimuth angle of the direction of flight at the perigee.
Definition: PerigeeParameters.h:39
Belle2::TrackFindingCDC::UncertainPerigeeCircle
Adds an uncertainty matrix to the circle in perigee parameterisation.
Definition: UncertainPerigeeCircle.h:39
Belle2::TrackFindingCDC::CDCObservations2D
Class serving as a storage of observed drift circles to present to the Riemann fitter.
Definition: CDCObservations2D.h:53
Belle2::TrackFindingCDC::UncertainPerigeeCircle::reverse
void reverse()
Flips the orientation of the circle in place.
Definition: UncertainPerigeeCircle.h:217
Belle2::TrackFindingCDC::UncertainPerigeeCircle::setNDF
void setNDF(std::size_t ndf)
Setter for the number of degrees of freediom used in the circle fit.
Definition: UncertainPerigeeCircle.h:202
Belle2::TrackFindingCDC::PerigeeCircle
Extension of the generalized circle also caching the perigee coordinates.
Definition: PerigeeCircle.h:46
Belle2::TrackFindingCDC::UncertainParametersUtil< PerigeeUtil, EPerigeeParameter >::covarianceFromPrecision
static CovarianceMatrix covarianceFromPrecision(const PrecisionMatrix &prec)
Convert the precision matrix to the corresponding covariance matrix.
Definition: UncertainParameters.icc.h:87
Belle2::TrackFindingCDC::Vector2D::x
double x() const
Getter for the x coordinate.
Definition: Vector2D.h:609
Belle2::TrackFindingCDC::CDCObservations2D::getNObservationsWithDriftRadius
std::size_t getNObservationsWithDriftRadius() const
Returns the number of observations having a drift radius radius.
Definition: CDCObservations2D.cc:316
Belle2::TrackFindingCDC::PlainMatrix
A matrix implementation to be used as an interface typ through out the track finder.
Definition: PlainMatrix.h:50
NParabolicParameterIndices
Namespace to hide the contained enum constants.
Definition: ExtendedRiemannsMethod.cc:73
Belle2::TrackFindingCDC::CDCTrajectory2D::clear
void clear()
Clears all information from this trajectoy.
Definition: CDCTrajectory2D.cc:90
Belle2::TrackFindingCDC::NPerigeeParameterIndices::c_Curv
@ c_Curv
Constant to address the curvature.
Definition: PerigeeParameters.h:36
Belle2::TrackFindingCDC::CDCObservations2D::getCentralPoint
Vector2D getCentralPoint() const
Extracts the observation center that is at the index in the middle.
Definition: CDCObservations2D.cc:330