9 #include <GaudiKernel/PhysicalConstants.h>
31 constexpr
double detCutOff = 1.e-8;
35 using namespace SegmentFit;
83 ATH_MSG_DEBUG(
"Somehow a measurement is on the narrow ridge of validity. Let's try if the time can be fitted now ");
98 fitResult.
nPhiMeas+= hit->measuresPhi();
99 fitResult.
nDoF+= hit->measuresPhi();
100 fitResult.
nDoF+= hit->measuresEta();
104 fitResult.
nTimeMeas+=hit->measuresTime();
106 if (!fitResult.
nDoF) {
116 return avgX += hit->positionInChamber().x() / nHits;
163 line.hessian[idxThetaSq] = -
Amg::Vector3D{phi.cs*theta.sn,phi.sn*theta.sn, theta.cs};
176 if (resObj.
projDirLenSq < std::numeric_limits<float>::epsilon()) {
196 const double lineDist = resObj.
projDir.cross(hitDir).dot(hitMinSeg);
197 const double resVal = (lineDist - hit.
driftRadius());
219 const double partialDist = resObj.
partProjDir[param].cross(hitDir).dot(hitMinSeg);
231 const double partialDist = - resObj.
projDir.cross(hitDir).dot(
line.gradient[param]);
252 for (
int param1 = param; param1>=0; --param1) {
256 const int lineIdx = vecIdxFromSymMat<nLinePars>(param, param1);
257 const int resIdx = vecIdxFromSymMat<toInt(ParamDefs::nPars)>(param, param1);
262 const double partSqLineProject =
line.hessian[lineIdx].dot(hitDir);
269 const double partialSqDist = projDirPartSq.cross(hitDir).dot(hitMinSeg);
285 const double partialSqDist = - resObj.
partProjDir[param].cross(hitDir).dot(
line.gradient[param1]);
305 const double planeOffSet = normal.dot(hitPos);
307 const double normDot = normal.dot(
line.dir);
308 if (std::abs(normDot) < std::numeric_limits<double>::epsilon()){
321 const double travelledDist = (planeOffSet -
line.pos.dot(normal)) / normDot;
325 residual.residual.block<2,1>(0,0) = (planeIsect - hitPos).block<2,1>(0,0);
327 for (
unsigned fitPar = 0; fitPar <
line.gradient.size(); ++fitPar) {
331 const double partialDist = - travelledDist / normDot * normal.dot(
line.gradient[fitPar]);
332 residual.gradient[fitPar].block<2,1>(0,0) = (travelledDist *
line.gradient[fitPar] + partialDist *
line.dir).block<2,1>(0,0);
336 residual.gradient[fitPar].block<2,1>(0,0) = (
line.gradient[fitPar] -
line.gradient[fitPar].dot(normal) / normDot *
line.dir).block<2,1>(0,0);
349 for (
int param1 = param; param1>=0; --param1) {
350 const int lineIdx = vecIdxFromSymMat<nLinePars>(param, param1);
351 const int resIdx = vecIdxFromSymMat<toInt(ParamDefs::nPars)>(param, param1);
354 residual.hessian[resIdx].block<2,1>(0,0) =(
355 travelledDist *
line.hessian[lineIdx] - travelledDist *(normal.dot(
line.hessian[lineIdx])) / normDot *
line.dir
356 - normal.dot(
line.gradient[param1]) / normDot *
residual.gradient[param]
357 - normal.dot(
line.gradient[param]) / normDot *
residual.gradient[param1]).block<2,1>(0,0);
359 const double gradientDisplace = normal.dot(
line.gradient[param1]);
360 if (gradientDisplace > std::numeric_limits<float>::epsilon()){
361 residual.hessian[resIdx].block<2,1>(0,0) = gradientDisplace*( normal.dot(
line.gradient[param]) /
std::pow(normDot,2)*
line.dir
362 -
line.gradient[param] / normDot ).block<2,1>(0,0);
380 std::stringstream hitStream{};
381 const auto [startPos, startDir] =
makeLine(startPars);
382 for (
const HitType& hit : calibHits) {
388 hitStream<<
", channel dir: "<<
Amg::toString(hit->directionInChamber())<<std::endl;
391 <<
", plane location: "<<
Amg::toString(localToGlobal)<<std::endl<<hitStream.str());
398 fitResult.calibMeasurements = std::move(calibHits);
404 const auto [segPos, segDir] =
makeLine(startPars);
415 unsigned int noChangeIter{0};
417 ATH_MSG_DEBUG(
"Iteration: "<<fitResult.nIter<<
" parameters: "<<
toString(fitResult.segmentPars)<<
", chi2: "<<fitResult.chi2);
431 for (
HitType& hit : fitResult.calibMeasurements) {
437 ATH_MSG_DEBUG(
"Update chi2 from measurement "<<(hit->spacePoint() ? idHelperSvc->toString(hit->spacePoint()->identify())
441 switch (hit->type()) {
445 const double dSign = (hit->driftRadius() > 0 ? 1. : -1.);
449 hit->setDriftRadius(dSign*hit->driftRadius());
453 if (!fitResult.timeFit) {
469 if (!fitResult.timeFit || !hit->measuresTime()) {
475 const double totFlightDist = (localToGlobal * planeIsect).
mag() * c_inv;
480 ATH_MSG_VERBOSE(
"Partial derivative of "<<idHelperSvc->toString(hit->spacePoint()->identify())
482 <<
" "<<std::endl<<
toString(hit->covariance())<<std::endl<<
" w.r.t "
490 const auto &measurement = *hit->spacePoint()->primaryMeasurement();
491 ATH_MSG_WARNING(
"MdtSegmentFitter() - Unsupported measurment type" <<
typeid(measurement).
name());
495 ATH_MSG_VERBOSE(
"Update derivatives for hit "<< (hit->spacePoint() ? idHelperSvc->toString(hit->spacePoint()->identify()) :
"beamspot"));
501 for (
int k =1;
k < 5 - (!fitResult.timeFit); ++
k){
502 for (
int l = 0;
l<
k; ++
l){
503 hessian(
l,
k) = hessian(
k,
l);
508 fitResult.converged =
true;
509 ATH_MSG_VERBOSE(
"Fit converged after "<<fitResult.nIter<<
" iterations with "<<fitResult.chi2);
515 if (!fitResult.nPhiMeas && !fitResult.timeFit) {
516 paramUpdate = updateParameters<2>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
517 }
else if (!fitResult.nPhiMeas && fitResult.timeFit) {
525 paramUpdate = updateParameters<3>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
531 }
else if (fitResult.nPhiMeas && !fitResult.timeFit) {
532 paramUpdate = updateParameters<4>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
533 }
else if (fitResult.nPhiMeas && fitResult.timeFit) {
534 paramUpdate = updateParameters<5>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
538 fitResult.converged =
true;
549 fitResult.nDoF-=fitResult.timeFit;
552 const auto [segPos, segDir] = fitResult.makeLine();
555 std::optional<double> toF = fitResult.timeFit ? std::make_optional<double>((localToGlobal * segPos).
mag() * c_inv) : std::nullopt;
557 std::ranges::stable_sort(fitResult.calibMeasurements, [](
const HitType&
a,
const HitType&
b){
558 return a->positionInChamber().z() < b->positionInChamber().z();
563 for (
const HitType& hit : fitResult.calibMeasurements) {
564 hit->setDriftRadius(std::abs(hit->driftRadius()));
568 if (!fitResult.nPhiMeas&& !fitResult.timeFit) {
569 blockCovariance<2>(std::move(hessian), fitResult.segmentParErrs);
570 }
else if (!fitResult.nPhiMeas && fitResult.timeFit) {
573 blockCovariance<3>(std::move(hessian), fitResult.segmentParErrs);
576 }
else if (fitResult.nPhiMeas) {
577 blockCovariance<4>(std::move(hessian), fitResult.segmentParErrs);
578 }
else if (fitResult.nPhiMeas && fitResult.timeFit) {
579 blockCovariance<5>(std::move(hessian), fitResult.segmentParErrs);
587 double&
chi2,
int startPar)
const {
591 for (
int p = startPar;
p >=0 ; --
p) {
592 gradient[
p] +=2.*covRes.dot(fitMeas.
gradient[
p]);
593 for (
int k=
p;
k>=0; --
k) {
596 const int symMatIdx = vecIdxFromSymMat<ResidualWithPartials::nPars>(
p,
k);
602 <<
toString(gradient)<<
", Hessian:\n"<<hessian<<
", measurement covariance\n"<<
toString(invCov));
605 template <
unsigned int nDim>
609 covariance.setIdentity();
610 AmgSymMatrix(nDim) miniHessian = hessian.block<nDim, nDim>(0,0);
611 if (std::abs(miniHessian.determinant()) <= detCutOff) {
612 ATH_MSG_VERBOSE(
"Boeser mini hessian ("<<miniHessian.determinant()<<
")\n"<<miniHessian<<
"\n\n"<<hessian);
615 ATH_MSG_VERBOSE(
"Hessian matrix: \n"<<hessian<<
",\nblock Hessian:\n"<<miniHessian<<
",\n determinant: "<<miniHessian.determinant());
616 covariance.block<nDim,nDim>(0,0) = miniHessian.inverse();
620 template <
unsigned int nDim>
626 AmgSymMatrix(nDim) miniHessian = currentHessian.block<nDim, nDim>(0,0);
628 <<
", hessian ("<<miniHessian.determinant()<<
")"<<std::endl<<miniHessian);
630 double changeMag{0.};
631 if (std::abs(miniHessian.determinant()) > detCutOff) {
632 prevPars.block<nDim,1>(0,0) = currPars.block<nDim,1>(0,0);
634 const AmgVector(nDim) updateMe = miniHessian.inverse()* currGrad.block<nDim, 1>(0,0);
635 changeMag = std::sqrt(updateMe.dot(updateMe));
636 currPars.block<nDim,1>(0,0) -= updateMe;
637 prevGrad.block<nDim,1>(0,0) = currGrad.block<nDim,1>(0,0);
638 ATH_MSG_DEBUG(
"Hessian inverse:\n"<<miniHessian.inverse()<<
"\n\nUpdate the parameters by -"
641 const AmgVector(nDim) gradDiff = (currGrad - prevGrad).block<nDim,1>(0,0);
642 const double gradDiffMag = gradDiff.mag2();
643 double denom = (gradDiffMag > std::numeric_limits<float>::epsilon() ? gradDiffMag : 1.);
644 const double gamma = std::abs((currPars - prevPars).block<nDim,1>(0,0).
dot(gradDiff))
646 ATH_MSG_VERBOSE(
"Hessian determinant invalid. Try deepest descent - \nprev parameters: "
648 prevPars.block<nDim, 1>(0,0) = currPars.block<nDim, 1>(0,0);
649 currPars.block<nDim, 1>(0,0) -=
gamma* currGrad.block<nDim, 1>(0,0);
650 prevGrad.block<nDim, 1>(0,0) = currGrad.block<nDim,1>(0,0);
651 changeMag = std::abs(
gamma) * currGrad.block<nDim, 1>(0,0).
mag();
654 unsigned int nOutOfBound{0};
655 for (
unsigned int p = 0;
p< nDim; ++
p) {
656 double& parValue{currPars[
p]};
657 if constexpr(nDim == 3) {