10 #include <tracking/trackFindingCDC/fitting/CDCSZFitter.h>
12 #include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
13 #include <tracking/trackFindingCDC/fitting/CDCSZObservations.h>
14 #include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
16 #include <tracking/trackFindingCDC/eventdata/tracks/CDCTrack.h>
17 #include <tracking/trackFindingCDC/eventdata/tracks/CDCSegmentPair.h>
19 #include <tracking/trackFindingCDC/eventdata/segments/CDCSegment3D.h>
20 #include <tracking/trackFindingCDC/eventdata/segments/CDCSegment2D.h>
22 #include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectorySZ.h>
23 #include <tracking/trackFindingCDC/eventdata/trajectories/CDCTrajectory2D.h>
25 #include <tracking/trackFindingCDC/geometry/SZParameters.h>
27 #include <tracking/trackFindingCDC/numerics/EigenView.h>
29 #include <framework/logging/Logger.h>
31 #include <Eigen/Eigen>
36 using namespace TrackFindingCDC;
48 Eigen::Matrix<double, 2, 2> sumMatrixWS = sumMatrixWSZ.block<2, 2>(0, 0);
49 Eigen::Matrix<double, 2, 1> sumVectorZOverWS = sumMatrixWSZ.block<2, 1>(0, 2);
50 Eigen::Matrix<double, 2, 1> nZOverWS = sumMatrixWS.llt().solve(sumVectorZOverWS);
51 double chi2 = sumMatrixWSZ(2, 2) - nZOverWS.transpose() * sumVectorZOverWS;
53 using namespace NSZParameterIndices;
55 szParameters(
c_TanL) = nZOverWS(1);
56 szParameters(
c_Z0) = nZOverWS(0);
62 szPrecision(
c_Z0,
c_Z0) = sumMatrixWS(0, 0);
69 UncertainSZLine fitSZ(
const Eigen::Matrix<double, 3, 3>& sumMatrixWSZ) __attribute__((__unused__));
72 UncertainSZLine fitSZ(
const Eigen::Matrix<double, 3, 3>& sumMatrixWSZ)
76 Eigen::Matrix<double, 3, 3> averageMatrixWSZ = sumMatrixWSZ / sumMatrixWSZ(0);
79 Eigen::Matrix<double, 3, 1> meansWSZ = averageMatrixWSZ.row(0);
82 Eigen::Matrix<double, 3, 3> covMatrixWSZ = averageMatrixWSZ - meansWSZ * meansWSZ.transpose();
84 Eigen::Matrix<double, 2, 2> covMatrixSZ = covMatrixWSZ.block<2, 2>(1, 1);
85 Eigen::Matrix<double, 2, 1> meansSZ = meansWSZ.segment<2>(1);
87 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 2, 2>> eigensolver(covMatrixSZ);
88 if (eigensolver.info() != Eigen::Success) {
90 "SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
96 Eigen::Matrix<double, 2, 1> nSZ = eigensolver.eigenvectors().col(0);
97 B2INFO(
"nSZ " << nSZ);
98 nSZ /= std::copysign(nSZ.norm(), -nSZ(1));
99 Eigen::Matrix<double, 3, 1> nWSZ;
101 nWSZ << -meansSZ.transpose() * nSZ, nSZ;
102 B2INFO(
"nWSZ " << nWSZ);
104 double chi2 = nWSZ.transpose() * sumMatrixWSZ * nWSZ;
106 Eigen::Matrix<double, 3, 3> precN = sumMatrixWSZ;
108 using namespace NSZParameterIndices;
109 SZParameters szParameters;
110 szParameters(
c_TanL) = -nWSZ(1) / nWSZ(2);
111 szParameters(
c_Z0) = -nWSZ(0) / nWSZ(2);
113 Eigen::Matrix<double, 3, 2> ambiguitySZToN = Eigen::Matrix<double, 3, 2>::Zero();
114 ambiguitySZToN(0,
c_Z0) = -nWSZ(2);
115 ambiguitySZToN(0,
c_TanL) = -nWSZ(1) * nWSZ(0);
116 ambiguitySZToN(1,
c_TanL) = -nWSZ(2) * nWSZ(2) * nWSZ(2);
117 ambiguitySZToN(2,
c_TanL) = nWSZ(1) * nWSZ(2) * nWSZ(2);
119 SZPrecision szPrecision;
120 mapToEigen(szPrecision) = ambiguitySZToN.transpose() * precN * ambiguitySZToN;
123 return UncertainSZLine(szParameters, szCovariance, chi2);
129 const bool onlyStereo =
true;
132 if (observationsSZ.
size() > 3) {
134 update(szTrajectory, observationsSZ);
144 B2ASSERT(
"Expected stereo segment", not stereoSegment.
isAxial());
147 update(trajectorySZ, stereoSegment, axialTrajectory2D);
155 return fit(std::move(observationsSZ));
161 update(trajectorySZ, observationsSZ);
170 assert(ptrStereoSegment);
171 assert(ptrAxialSegment);
178 update(trajectorySZ, stereoSegment, axialTrajectory2D);
187 for (
size_t i = 0; i < observations2D.
size(); ++i) {
188 const double s = observations2D.
getX(i);
189 const double z = observations2D.
getY(i);
190 szObservations.
fill(s, z);
192 return fit(std::move(szObservations));
199 B2ASSERT(
"Expected stereo segment", not stereoSegment.
isAxial());
205 observationsSZ.
append(recoHit3D);
208 update(trajectorySZ, observationsSZ);
213 trajectorySZ.
clear();
214 if (observationsSZ.
size() < 3) {
219 size_t ndf = observationsSZ.
size() - 2;
222 Eigen::Matrix<double, 3, 3> sumMatrixWSZ = getWSZSumMatrix(observationsSZ);
225 uncertainSZLine.
setNDF(ndf);