10 #include <tracking/trackFindingCDC/fitting/FacetFitter.h>
12 #include <tracking/trackFindingCDC/eventdata/hits/CDCFacet.h>
13 #include <tracking/trackFindingCDC/eventdata/hits/CDCWireHit.h>
14 #include <tracking/trackFindingCDC/geometry/UncertainParameterLine2D.h>
15 #include <tracking/trackFindingCDC/geometry/ParameterLine2D.h>
16 #include <tracking/trackFindingCDC/geometry/Vector2D.h>
18 #include <tracking/trackFindingCDC/numerics/EigenView.h>
22 #include <Math/Functor.h>
23 #include <Math/BrentMinimizer1D.h>
26 using namespace TrackFindingCDC;
31 Vector2D getCenterForwardDirection(
const Matrix<double, N, 3>& xyl)
34 Vector2D coordinate(xyl(N - 1, 0) - xyl(0, 0), xyl(N - 1, 1) - xyl(0, 1));
35 return coordinate.unit();
39 Vector2D getTangentialForwardDirection(
const Matrix<double, N, 3>& xyl)
42 Vector2D fromPos(xyl(0, 0), xyl(0, 1));
43 double fromL = xyl(0, 2);
45 Vector2D toPos(xyl(N - 1, 0), xyl(N - 1, 1));
46 double toL = xyl(N - 1, 2);
49 Vector2D coordinate = tangentLine.tangential();
50 return coordinate.unit();
54 void rotate(Vector2D coordinate, Matrix<double, N, 3>& xyl)
57 rot(0, 0) = coordinate.x();
58 rot(0, 1) = -coordinate.y();
59 rot(1, 0) = coordinate.y();
60 rot(1, 1) = coordinate.x();
65 void unrotate(Vector2D coordinate, Vector2D& vec)
69 vec = vec.passiveRotatedBy(coordinate.flippedSecond());
72 Eigen::Vector2d fitPhiVecZeroSteps(
const Eigen::Matrix<double, 3, 3>& xylCov,
double& chi2)
74 chi2 = xylCov(1, 1) + 2 * xylCov(1, 2) + xylCov(2, 2);
75 return Eigen::Vector2d(1, 0);
78 Eigen::Vector2d fitPhiVecOneStep(
const Eigen::Matrix<double, 3, 3>& xylCov,
double& chi2)
80 const double phi = (xylCov(0, 1) + xylCov(0, 2)) / xylCov(0, 0);
81 chi2 = xylCov(1, 1) + 2 * xylCov(1, 2) + xylCov(2, 2) - phi * (xylCov(0, 1) + xylCov(0, 2));
82 return Eigen::Vector2d(std::cos(phi), std::sin(phi));
85 Eigen::Vector2d fitPhiVecBrent(
const Eigen::Matrix<double, 3, 3>& xylCov,
int nIter,
double& chi2)
87 const Eigen::Matrix< double, 2, 2> A = xylCov.topLeftCorner<2, 2>();
88 const Eigen::Matrix< double, 2, 1> b = xylCov.topRightCorner<2, 1>();
89 const double c = xylCov(2, 2);
91 auto calcReducedChi2 = [&A, &b](
double phi) ->
double {
92 Eigen::Matrix<double, 2, 1> normal(std::sin(phi), -std::cos(phi));
93 return ((normal.transpose() * A - 2 * b.transpose()) * normal)[0];
96 ROOT::Math::Functor1D functor(calcReducedChi2);
97 ROOT::Math::BrentMinimizer1D bm;
98 bm.SetFunction(functor, -M_PI / 2, M_PI / 2);
101 chi2 = bm.FValMinimum() + c;
102 const double phi = bm.XMinimum();
103 return Eigen::Vector2d(std::cos(phi), std::sin(phi));
107 UncertainParameterLine2D fit(Matrix<double, N, 3> xylIn,
108 Matrix<double, N, 1> wIn,
112 Vector2D coordinate = getTangentialForwardDirection(xylIn);
115 if (coordinate.hasNAN()) {
116 coordinate = getCenterForwardDirection(xylIn);
119 rotate(coordinate, xylIn);
121 auto xyl = mapToEigen(xylIn);
122 auto w = mapToEigen(wIn).array();
124 Eigen::Array< double, 1, 3> averages = (xyl.array().colwise() * w).colwise().sum() / w.sum();
125 Eigen::Matrix< double, N, 3> deltas = xyl.array().rowwise() - averages;
126 Eigen::Matrix< double, N, 3> weightedDeltas = deltas.array().colwise() * w;
127 Eigen::Matrix< double, 3, 3> covariances = deltas.transpose() * weightedDeltas / w.sum();
129 Eigen::Vector2d phiVec;
132 phiVec = fitPhiVecZeroSteps(covariances, chi2);
133 }
else if (nSteps == 1) {
134 phiVec = fitPhiVecOneStep(covariances, chi2);
136 phiVec = fitPhiVecBrent(covariances, nSteps, chi2);
140 double meanArcLength = averages.topLeftCorner<1, 2>().matrix() * phiVec;
141 double varArcLength = phiVec.transpose() * covariances.topLeftCorner<2, 2>() * phiVec;
144 using namespace NLineParameterIndices;
145 LinePrecision linePrecision;
146 linePrecision(
c_Phi0,
c_Phi0) = p * (varArcLength + meanArcLength * meanArcLength);
147 linePrecision(
c_Phi0,
c_I) = p * meanArcLength;
148 linePrecision(
c_I,
c_Phi0) = p * meanArcLength;
149 linePrecision(
c_I,
c_I) = p;
152 Vector2D tangential(phiVec(0), phiVec(1));
153 Vector2D n12 = tangential.orthogonal(ERotation::c_Clockwise);
154 double n0 = averages(2) - averages(0) * n12.x() - averages(1) * n12.y();
155 Vector2D support = -n12 * n0;
158 unrotate(coordinate, support);
159 unrotate(coordinate, tangential);
161 ParameterLine2D parameterLine2D(support, tangential);
163 return UncertainParameterLine2D(parameterLine2D, lineCovariance, chi2, ndf);
176 const CDCRLWireHit& startRLWireHit = facet.getStartRLWireHit();
177 const CDCRLWireHit& middleRLWireHit = facet.getMiddleRLWireHit();
178 const CDCRLWireHit& endRLWireHit = facet.getEndRLWireHit();
180 const Vector2D support = middleRLWireHit.getWireHit().getRefPos2D();
182 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
183 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
184 xyl(0, 0) = startWirePos2D.x() - support.x();
185 xyl(0, 1) = startWirePos2D.y() - support.y();
186 xyl(0, 2) = startRLWireHit.getSignedRefDriftLength();
187 w(0) = 1.0 / startDriftLengthVar;
189 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
190 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
191 xyl(1, 0) = middleWirePos2D.x() - support.x();
192 xyl(1, 1) = middleWirePos2D.y() - support.y();
193 xyl(1, 2) = middleRLWireHit.getSignedRefDriftLength();
194 w(1) = 1.0 / middleDriftLengthVar;
196 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
197 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
198 xyl(2, 0) = endWirePos2D.x() - support.x();
199 xyl(2, 1) = endWirePos2D.y() - support.y();
200 xyl(2, 2) = endRLWireHit.getSignedRefDriftLength();
201 w(2) = 1.0 / endDriftLengthVar;
203 UncertainParameterLine2D fitLine{
::fit(std::move(xyl), std::move(w), nSteps) };
204 fitLine.passiveMoveBy(-support);
205 facet.setFitLine(fitLine);
206 return fitLine.chi2();
211 const CDCFacet& toFacet,
220 const Vector2D support =
Vector2D::average(fromFacet.getMiddleWireHit().getRefPos2D(),
221 toFacet.getMiddleWireHit().getRefPos2D());
223 const CDCRLWireHit& startRLWireHit = fromFacet.getStartRLWireHit();
224 const CDCRLWireHit& middleRLWireHit = fromFacet.getMiddleRLWireHit();
225 const CDCRLWireHit& endRLWireHit = fromFacet.getEndRLWireHit();
227 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
228 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
229 xyl(0, 0) = startWirePos2D.x() - support.x();
230 xyl(0, 1) = startWirePos2D.y() - support.y();
231 xyl(0, 2) = startRLWireHit.getSignedRefDriftLength();
232 w(0) = 1.0 / startDriftLengthVar;
234 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
235 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
236 xyl(1, 0) = middleWirePos2D.x() - support.x();
237 xyl(1, 1) = middleWirePos2D.y() - support.y();
238 xyl(1, 2) = middleRLWireHit.getSignedRefDriftLength();
239 w(1) = 1.0 / middleDriftLengthVar;
241 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
242 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
243 xyl(2, 0) = endWirePos2D.x() - support.x();
244 xyl(2, 1) = endWirePos2D.y() - support.y();
245 xyl(2, 2) = endRLWireHit.getSignedRefDriftLength();
246 w(2) = 1.0 / endDriftLengthVar;
250 const CDCRLWireHit& startRLWireHit = toFacet.getStartRLWireHit();
251 const CDCRLWireHit& middleRLWireHit = toFacet.getMiddleRLWireHit();
252 const CDCRLWireHit& endRLWireHit = toFacet.getEndRLWireHit();
254 const double startDriftLengthVar = startRLWireHit.getRefDriftLengthVariance();
255 const Vector2D startWirePos2D = startRLWireHit.getWireHit().getRefPos2D();
256 xyl(3, 0) = startWirePos2D.x() - support.x();
257 xyl(3, 1) = startWirePos2D.y() - support.y();
258 xyl(3, 2) = startRLWireHit.getSignedRefDriftLength();
259 w(3) = 1.0 / startDriftLengthVar;
261 const double middleDriftLengthVar = middleRLWireHit.getRefDriftLengthVariance();
262 const Vector2D middleWirePos2D = middleRLWireHit.getWireHit().getRefPos2D();
263 xyl(4, 0) = middleWirePos2D.x() - support.x();
264 xyl(4, 1) = middleWirePos2D.y() - support.y();
265 xyl(4, 2) = middleRLWireHit.getSignedRefDriftLength();
266 w(4) = 1.0 / middleDriftLengthVar;
268 const double endDriftLengthVar = endRLWireHit.getRefDriftLengthVariance();
269 const Vector2D endWirePos2D = endRLWireHit.getWireHit().getRefPos2D();
270 xyl(5, 0) = endWirePos2D.x() - support.x();
271 xyl(5, 1) = endWirePos2D.y() - support.y();
272 xyl(5, 2) = endRLWireHit.getSignedRefDriftLength();
273 w(5) = 1.0 / endDriftLengthVar;
276 UncertainParameterLine2D fitLine{
::fit(std::move(xyl), std::move(w), nSteps) };
277 fitLine.passiveMoveBy(-support);
283 Matrix<double, 3, 1> w,
286 return ::fit(std::move(xyl), std::move(w), nSteps);