8#include <tracking/trackFindingCDC/fitting/RiemannsMethod.h>
10#include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
11#include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
13#include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectory2D.h>
15#include <framework/logging/Logger.h>
23using namespace TrackFindingCDC;
26 : m_lineConstrained(false)
27 , m_originConstrained(false)
42 if (isCoaligned == EForwardBackward::c_Backward) trajectory2D.
reverse();
50 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
51 assert(eigenObservation.cols() == 4);
52 size_t nObservations = observations2D.
size();
57 Eigen::Matrix<double, Eigen::Dynamic, 2> points = eigenObservation.leftCols<2>();
59 Eigen::Matrix<double, 1, 2> pointMean;
62 pointMean << 0.0, 0.0;
65 pointMean = points.colwise().mean();
67 points = points.rowwise() - pointMean;
69 Eigen::Matrix<double, 2, 2> covarianceMatrix = points.transpose() * points;
71 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(covarianceMatrix);
73 if (eigensolver.info() != Eigen::Success)
74 B2WARNING(
"SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
79 Eigen::Matrix<double, 2, 1> normalToLine = eigensolver.eigenvectors().col(0);
81 double offset = -pointMean * normalToLine;
90 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
92 projectedPoints.col(0) = eigenObservation.col(0);
93 projectedPoints.col(1) = eigenObservation.col(1);
94 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm();
97 Eigen::Matrix<double, 1, 3> pointMean;
99 pointMean << 0.0, 0.0, 0.0;
102 pointMean = projectedPoints.colwise().mean();
104 projectedPoints = projectedPoints.rowwise() - pointMean;
107 Eigen::Matrix<double, 3, 3> covarianceMatrix = projectedPoints.transpose() * projectedPoints;
109 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(covarianceMatrix);
111 if (eigensolver.info() != Eigen::Success)
112 B2WARNING(
"Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
117 Eigen::Matrix<double, 3, 1> normalToPlane = eigensolver.eigenvectors().col(0);
119 double offset = -pointMean * normalToPlane;
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;
138 if (voteForChangeSign > nObservations / 2.0) trajectory2D.
reverse();
146 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
147 assert(eigenObservation.cols() == 4);
148 size_t nObservations = observations2D.
size();
158 Eigen::Matrix< double, Eigen::Dynamic, 1 > distances = eigenObservation.col(2);
165 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 2);
169 projectedPoints.col(0) = eigenObservation.col(0);
170 projectedPoints.col(1) = eigenObservation.col(1);
173 Eigen::Matrix<double, 2, 1> parameters =
174 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
181 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
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();
189 Eigen::Matrix<double, 3, 1> parameters =
190 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
197 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
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);
205 Eigen::Matrix<double, 3, 1> parameters =
206 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
215 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 4);
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();
225 Eigen::Matrix<double, 4, 1> parameters =
226 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
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.
RiemannsMethod()
Default constructor.
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...
double x() const
Getter for the x coordinate.
double y() const
Getter for the y coordinate.
EForwardBackward
Enumeration to represent the distinct possibilities of the right left passage information.
Abstract base class for different kinds of events.