10 #include <tracking/trackFindingCDC/fitting/RiemannsMethod.h>
12 #include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
13 #include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
15 #include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectory2D.h>
17 #include <framework/logging/Logger.h>
19 #include <Eigen/Eigen>
25 using namespace TrackFindingCDC;
28 : m_lineConstrained(false)
29 , m_originConstrained(false)
44 if (isCoaligned == EForwardBackward::c_Backward) trajectory2D.
reverse();
52 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
53 assert(eigenObservation.cols() == 4);
54 size_t nObservations = observations2D.
size();
59 Eigen::Matrix<double, Eigen::Dynamic, 2> points = eigenObservation.leftCols<2>();
61 Eigen::Matrix<double, 1, 2> pointMean;
64 pointMean << 0.0, 0.0;
67 pointMean = points.colwise().mean();
69 points = points.rowwise() - pointMean;
71 Eigen::Matrix<double, 2, 2> covarianceMatrix = points.transpose() * points;
73 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(covarianceMatrix);
75 if (eigensolver.info() != Eigen::Success)
76 B2WARNING(
"SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
81 Eigen::Matrix<double, 2, 1> normalToLine = eigensolver.eigenvectors().col(0);
83 double offset = -pointMean * normalToLine;
92 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
94 projectedPoints.col(0) = eigenObservation.col(0);
95 projectedPoints.col(1) = eigenObservation.col(1);
96 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm();
99 Eigen::Matrix<double, 1, 3> pointMean;
101 pointMean << 0.0, 0.0, 0.0;
104 pointMean = projectedPoints.colwise().mean();
106 projectedPoints = projectedPoints.rowwise() - pointMean;
109 Eigen::Matrix<double, 3, 3> covarianceMatrix = projectedPoints.transpose() * projectedPoints;
111 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(covarianceMatrix);
113 if (eigensolver.info() != Eigen::Success)
114 B2WARNING(
"Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
119 Eigen::Matrix<double, 3, 1> normalToPlane = eigensolver.eigenvectors().col(0);
121 double offset = -pointMean * normalToPlane;
133 size_t voteForChangeSign = 0;
134 for (
size_t iPoint = 0; iPoint < nObservations; ++iPoint) {
135 double pointInSameDirection = eigenObservation(iPoint, 0) * directionAtCenter.
x() +
136 eigenObservation(iPoint, 1) * directionAtCenter.
y();
137 if (pointInSameDirection < 0) ++voteForChangeSign;
140 if (voteForChangeSign > nObservations / 2.0) trajectory2D.
reverse();
148 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
149 assert(eigenObservation.cols() == 4);
150 size_t nObservations = observations2D.
size();
160 Eigen::Matrix< double, Eigen::Dynamic, 1 > distances = eigenObservation.col(2);
167 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 2);
171 projectedPoints.col(0) = eigenObservation.col(0);
172 projectedPoints.col(1) = eigenObservation.col(1);
175 Eigen::Matrix<double, 2, 1> parameters =
176 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
183 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
187 projectedPoints.col(0) = eigenObservation.col(0);
188 projectedPoints.col(1) = eigenObservation.col(1);
189 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
191 Eigen::Matrix<double, 3, 1> parameters =
192 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
199 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
202 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
203 projectedPoints.col(1) = eigenObservation.col(0);
204 projectedPoints.col(2) = eigenObservation.col(1);
207 Eigen::Matrix<double, 3, 1> parameters =
208 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
217 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 4);
220 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
221 projectedPoints.col(1) = eigenObservation.col(0);
222 projectedPoints.col(2) = eigenObservation.col(1);
223 projectedPoints.col(3) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
227 Eigen::Matrix<double, 4, 1> parameters =
228 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);