10 #include <tracking/trackFindingCDC/fitting/ExtendedRiemannsMethod.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 <tracking/trackFindingCDC/geometry/UncertainPerigeeCircle.h>
18 #include <tracking/trackFindingCDC/geometry/PerigeeParameters.h>
19 #include <tracking/trackFindingCDC/geometry/Vector2D.h>
21 #include <tracking/trackFindingCDC/numerics/EigenView.h>
23 #include <framework/logging/Logger.h>
25 #include <Eigen/Eigen>
28 using namespace Belle2::TrackFindingCDC;
31 : m_lineConstrained(false)
32 , m_originConstrained(false)
39 size_t nObservations = observations2D.
size();
41 if (not nObservations)
return;
51 double frontX = observations2D.
getX(0);
52 double frontY = observations2D.
getY(0);
55 double backX = observations2D.
getX(nObservations - 1);
56 double backY = observations2D.
getY(nObservations - 1);
75 enum EParabolicIndices {
84 PerigeeCircle fit(
const Eigen::Matrix< double, 5, 5 >& sumMatrix,
85 bool lineConstrained =
false,
86 bool originConstrained =
false)
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);
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);
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);
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);
125 bool lineConstrained =
false,
126 bool originConstrained =
false)
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");
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");
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");
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");
176 PerigeeCircle fitSeperateOffset(Eigen::Matrix< double, 4, 1 > means,
177 Eigen::Matrix< double, 4, 4 > c,
178 bool lineConstrained =
false)
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");
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);
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");
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);
219 const Eigen::Matrix< double, 5, 5 >& s)
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();
228 double chi2 = n.transpose() * s * n;
235 const Eigen::Matrix< double, 4, 4 >& s)
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();
243 double chi2 = n.transpose() * s * n;
249 const Eigen::Matrix< double, 4, 4 >& s,
250 bool lineConstrained =
false,
251 bool originConstrained =
false)
253 const double impact = parameters.impact();
254 const Vector2D& phi0Vec = parameters.tangential();
255 const double curvature = parameters.curvature();
257 using namespace NPerigeeParameterIndices;
258 Eigen::Matrix<double, 4, 3> ambiguity = Eigen::Matrix<double, 4, 3> ::Zero();
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;
266 ambiguity(1,
c_Phi0) = phi0Vec.
x() * (1 + curvature * impact);
267 ambiguity(2,
c_Phi0) = phi0Vec.
y() * (1 + curvature * impact);
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;
275 Eigen::Matrix<double, 3, 3> perigeePrecision = ambiguity.transpose() * s * ambiguity;
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();
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();
288 mapToEigen(result) = perigeePrecision;
301 Eigen::Matrix< double, 5, 5 > s = getWXYRLSumMatrix(observations2D);
304 Eigen::Matrix<double, 4, 4> sNoL = s.block<4, 4>(0, 0);
307 size_t ndf = observations2D.
size() - 1;
322 if (nObservationsWithDriftRadius > 0) {
324 chi2 = calcChi2(resultCircle, s);
330 Eigen::Matrix< double, 4, 4> aNoL = sNoL / sNoL(iW);
333 Eigen::Matrix< double, 4, 1> meansNoL = aNoL.row(iW);
336 Eigen::Matrix< double, 4, 4> cNoL = aNoL - meansNoL * meansNoL.transpose();
344 chi2 = calcChi2(resultCircle, sNoL);