Belle II Software development
RiemannsMethod.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/RiemannsMethod.h>
9
10#include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
11#include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
12
13#include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectory2D.h>
14
15#include <framework/logging/Logger.h>
16
17#include <Eigen/Eigen>
18#include <Eigen/Core>
19
20#include <cassert>
21
22using namespace Belle2;
23using namespace TrackFindingCDC;
24
26 : m_lineConstrained(false)
27 , m_originConstrained(false)
28{
29}
30
32 CDCObservations2D& observations2D) const
33{
34
35 if (observations2D.getNObservationsWithDriftRadius() > 0) {
36 updateWithDriftLength(trajectory2D, observations2D);
37 } else {
38 updateWithoutDriftLength(trajectory2D, observations2D);
39 }
40
41 EForwardBackward isCoaligned = observations2D.isCoaligned(trajectory2D);
42 if (isCoaligned == EForwardBackward::c_Backward) trajectory2D.reverse();
43}
44
45
46
48 CDCObservations2D& observations2D) const
49{
50 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
51 assert(eigenObservation.cols() == 4);
52 size_t nObservations = observations2D.size();
53
54 if (isLineConstrained()) {
55
56 //do a normal line fit
57 Eigen::Matrix<double, Eigen::Dynamic, 2> points = eigenObservation.leftCols<2>();
58
59 Eigen::Matrix<double, 1, 2> pointMean;
60 //RowVector2d pointMean;
61 // cppcheck-suppress constStatement
62 pointMean << 0.0, 0.0;
63 if (!(isOriginConstrained())) {
64 // subtract the offset from the origin
65 pointMean = points.colwise().mean();
66
67 points = points.rowwise() - pointMean;
68 }
69 Eigen::Matrix<double, 2, 2> covarianceMatrix = points.transpose() * points;
70
71 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(covarianceMatrix);
72
73 if (eigensolver.info() != Eigen::Success)
74 B2WARNING("SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
75
76 //the eigenvalues are generated in increasing order
77 //we are interested in the lowest one since we want to compute the normal vector of the plane
78
79 Eigen::Matrix<double, 2, 1> normalToLine = eigensolver.eigenvectors().col(0);
80
81 double offset = -pointMean * normalToLine;
82
83 // set the generalized circle parameters
84 // last set to zero constrains to a line
85 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(offset, normalToLine(0), normalToLine(1), 0)));
86
87 } else {
88
89 //lift the points to the projection space
90 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
91
92 projectedPoints.col(0) = eigenObservation.col(0);
93 projectedPoints.col(1) = eigenObservation.col(1);
94 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm();
95
96
97 Eigen::Matrix<double, 1, 3> pointMean;
98 // cppcheck-suppress constStatement
99 pointMean << 0.0, 0.0, 0.0;
100 if (!(isOriginConstrained())) {
101 // subtract the offset from the origin
102 pointMean = projectedPoints.colwise().mean();
103
104 projectedPoints = projectedPoints.rowwise() - pointMean;
105 }
106
107 Eigen::Matrix<double, 3, 3> covarianceMatrix = projectedPoints.transpose() * projectedPoints;
108
109 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(covarianceMatrix);
110
111 if (eigensolver.info() != Eigen::Success)
112 B2WARNING("Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
113
114 //the eigenvalues are generated in increasing order
115 //we are interested in the lowest one since we want to compute the normal vector of the plane
116
117 Eigen::Matrix<double, 3, 1> normalToPlane = eigensolver.eigenvectors().col(0);
118
119 double offset = -pointMean * normalToPlane;
120
121 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(offset, normalToPlane(0), normalToPlane(1),
122 normalToPlane(2))));
123 //fit.setParameters();
124
125 }
126
127 //check if the orientation is alright
128 Vector2D directionAtCenter = trajectory2D.getFlightDirection2D(Vector2D(0.0, 0.0));
129
130
131 size_t voteForChangeSign = 0;
132 for (size_t iPoint = 0; iPoint < nObservations; ++iPoint) {
133 double pointInSameDirection = eigenObservation(iPoint, 0) * directionAtCenter.x() +
134 eigenObservation(iPoint, 1) * directionAtCenter.y();
135 if (pointInSameDirection < 0) ++voteForChangeSign;
136 }
137
138 if (voteForChangeSign > nObservations / 2.0) trajectory2D.reverse();
139
140}
141
142
143
145{
146 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
147 assert(eigenObservation.cols() == 4);
148 size_t nObservations = observations2D.size();
149
150 //cout << "updateWithRightLeft : " << endl;
151 //observations always have the structure
152 /*
153 observables[0][0] == x of first point
154 observables[0][1] == y of first point
155 observables[0][2] == desired distance of first point
156 */
157
158 Eigen::Matrix< double, Eigen::Dynamic, 1 > distances = eigenObservation.col(2);
159
160 //cout << "distances : " << endl << distances << endl;
161
162 if ((isLineConstrained()) && (isOriginConstrained())) {
163
164
165 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 2);
166
167 //all coordinates
168 //projectedPoints.col(0) = Eigen::Matrix<double,Eigen::Dynamic,1>::Constant(nObservations,1.0);
169 projectedPoints.col(0) = eigenObservation.col(0);
170 projectedPoints.col(1) = eigenObservation.col(1);
171 //projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
172
173 Eigen::Matrix<double, 2, 1> parameters =
174 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
175
176 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(0.0, parameters(0), parameters(1), 0.0)));
177 }
178
179 else if ((! isLineConstrained()) && (isOriginConstrained())) {
180
181 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
182
183 //all coordinates
184 //projectedPoints.col(0) = Eigen::Matrix<double,Eigen::Dynamic,1>::Constant(1.0);
185 projectedPoints.col(0) = eigenObservation.col(0);
186 projectedPoints.col(1) = eigenObservation.col(1);
187 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
188
189 Eigen::Matrix<double, 3, 1> parameters =
190 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
191
192 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(0.0, parameters(0), parameters(1), parameters(2))));
193 }
194
195 else if ((isLineConstrained()) && (! isOriginConstrained())) {
196
197 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
198
199 //all coordinates
200 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
201 projectedPoints.col(1) = eigenObservation.col(0);
202 projectedPoints.col(2) = eigenObservation.col(1);
203 //projectedPoints.col(3) = eigenObservation.leftCols<2>().rowwise().squaredNorm()- distances.rowwise().squaredNorm();
204
205 Eigen::Matrix<double, 3, 1> parameters =
206 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
207
208 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(parameters(0), parameters(1), parameters(2), 0.0)));
209 //fit.setParameters(parameters(0),parameters(1),parameters(2),0.0);
210
211 }
212
213 else if ((! isLineConstrained()) && (! isOriginConstrained())) {
214
215 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 4);
216
217 //all coordinates
218 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
219 projectedPoints.col(1) = eigenObservation.col(0);
220 projectedPoints.col(2) = eigenObservation.col(1);
221 projectedPoints.col(3) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
222
223 //cout << "points : " << endl << projectedPoints << endl;
224
225 Eigen::Matrix<double, 4, 1> parameters =
226 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
227
228 trajectory2D.setGlobalCircle(UncertainPerigeeCircle(PerigeeCircle::fromN(parameters(0), parameters(1), parameters(2),
229 parameters(3))));
230
231 }
232}
Class serving as a storage of observed drift circles to present to the Riemann fitter.
EForwardBackward isCoaligned(const CDCTrajectory2D &trajectory2D) const
Checks if the last observation in the vector lies at greater or lower travel distance than the last o...
std::size_t size() const
Returns the number of observations stored.
std::size_t getNObservationsWithDriftRadius() const
Returns the number of observations having a drift radius radius.
Particle trajectory as it is seen in xy projection represented as a circle.
void reverse()
Reverses the trajectory in place.
void setGlobalCircle(const UncertainPerigeeCircle &perigeeCircle)
Setter for the generalized circle that describes the trajectory.
Vector2D getFlightDirection2D(const Vector2D &point) const
Get the unit direction of flight at the given point, where arcLength2D = 0.
static PerigeeCircle fromN(double n0, double n1, double n2, double n3=0)
Constructor with the four parameters of the generalized circle.
void updateWithDriftLength(CDCTrajectory2D &trajectory2D, CDCObservations2D &observations2D) const
Executes the fit using the drift length information.
bool isLineConstrained() const
Getter for the indicator that lines should be fitted by this fitter.
void updateWithoutDriftLength(CDCTrajectory2D &trajectory2D, CDCObservations2D &observations2D) const
Executes the fit without using the drift length information.
void update(CDCTrajectory2D &trajectory2D, CDCObservations2D &observations2D) const
Executes the fit and updates the trajectory parameters. This may render the information in the observ...
bool isOriginConstrained() const
Getter for the indicator that curves through the origin should be fitted by this fitter.
Adds an uncertainty matrix to the circle in perigee parameterisation.
A two dimensional vector which is equipped with functions for correct handling of orientation relate...
Definition: Vector2D.h:32
double x() const
Getter for the x coordinate.
Definition: Vector2D.h:595
double y() const
Getter for the y coordinate.
Definition: Vector2D.h:605
EForwardBackward
Enumeration to represent the distinct possibilities of the right left passage information.
Abstract base class for different kinds of events.