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) {
158 line.hessian[idxThetaSq] = -
Amg::Vector3D{phi.cs*theta.sn,phi.sn*theta.sn, theta.cs};
171 if (resObj.
projDirLenSq < std::numeric_limits<float>::epsilon()) {
191 const double lineDist = resObj.
projDir.cross(hitDir).dot(hitMinSeg);
192 const double resVal = (lineDist - hit.
driftRadius());
214 const double partialDist = resObj.
partProjDir[param].cross(hitDir).dot(hitMinSeg);
226 const double partialDist = - resObj.
projDir.cross(hitDir).dot(
line.gradient[param]);
247 for (
int param1 = param; param1>=0; --param1) {
251 const int lineIdx = vecIdxFromSymMat<nLinePars>(param, param1);
252 const int resIdx = vecIdxFromSymMat<toInt(ParamDefs::nPars)>(param, param1);
257 const double partSqLineProject =
line.hessian[lineIdx].dot(hitDir);
264 const double partialSqDist = projDirPartSq.cross(hitDir).dot(hitMinSeg);
280 const double partialSqDist = - resObj.
partProjDir[param].cross(hitDir).dot(
line.gradient[param1]);
300 const double planeOffSet = normal.dot(hitPos);
302 const double normDot = normal.dot(
line.dir);
303 if (std::abs(normDot) < std::numeric_limits<double>::epsilon()){
316 const double travelledDist = (planeOffSet -
line.pos.dot(normal)) / normDot;
320 residual.residual.block<2,1>(0,0) = (planeIsect - hitPos).block<2,1>(0,0);
322 for (
unsigned fitPar = 0; fitPar <
line.gradient.size(); ++fitPar) {
326 const double partialDist = - travelledDist / normDot * normal.dot(
line.gradient[fitPar]);
327 residual.gradient[fitPar].block<2,1>(0,0) = (travelledDist *
line.gradient[fitPar] + partialDist *
line.dir).block<2,1>(0,0);
331 residual.gradient[fitPar].block<2,1>(0,0) = (
line.gradient[fitPar] -
line.gradient[fitPar].dot(normal) / normDot *
line.dir).block<2,1>(0,0);
344 for (
int param1 = param; param1>=0; --param1) {
345 const int lineIdx = vecIdxFromSymMat<nLinePars>(param, param1);
346 const int resIdx = vecIdxFromSymMat<toInt(ParamDefs::nPars)>(param, param1);
349 residual.hessian[resIdx].block<2,1>(0,0) =(
350 travelledDist *
line.hessian[lineIdx] - travelledDist *(normal.dot(
line.hessian[lineIdx])) / normDot *
line.dir
351 - normal.dot(
line.gradient[param1]) / normDot *
residual.gradient[param]
352 - normal.dot(
line.gradient[param]) / normDot *
residual.gradient[param1]).block<2,1>(0,0);
354 const double gradientDisplace = normal.dot(
line.gradient[param1]);
355 if (gradientDisplace > std::numeric_limits<float>::epsilon()){
356 residual.hessian[resIdx].block<2,1>(0,0) = gradientDisplace*( normal.dot(
line.gradient[param]) /
std::pow(normDot,2)*
line.dir
357 -
line.gradient[param] / normDot ).block<2,1>(0,0);
375 std::stringstream hitStream{};
376 const auto [startPos, startDir] =
makeLine(startPars);
377 for (
const HitType& hit : calibHits) {
383 hitStream<<
", channel dir: "<<
Amg::toString(hit->directionInChamber())<<std::endl;
386 <<
", plane location: "<<
Amg::toString(localToGlobal)<<std::endl<<hitStream.str());
393 fitResult.calibMeasurements = std::move(calibHits);
399 const auto [segPos, segDir] =
makeLine(startPars);
410 unsigned int noChangeIter{0};
412 ATH_MSG_DEBUG(
"Iteration: "<<fitResult.nIter<<
" parameters: "<<
toString(fitResult.segmentPars)<<
", chi2: "<<fitResult.chi2);
426 for (
HitType& hit : fitResult.calibMeasurements) {
432 ATH_MSG_DEBUG(
"Update chi2 from measurement "<<(hit->spacePoint() ? idHelperSvc->toString(hit->spacePoint()->identify())
436 switch (hit->type()) {
440 const double dSign = (hit->driftRadius() > 0 ? 1. : -1.);
444 hit->setDriftRadius(dSign*hit->driftRadius());
448 if (!fitResult.timeFit) {
464 if (!fitResult.timeFit || !hit->measuresTime()) {
470 const double totFlightDist = (localToGlobal * planeIsect).
mag() * c_inv;
475 ATH_MSG_VERBOSE(
"Partial derivative of "<<idHelperSvc->toString(hit->spacePoint()->identify())
477 <<
" "<<std::endl<<
toString(hit->covariance())<<std::endl<<
" w.r.t "
485 const auto &measurement = *hit->spacePoint()->primaryMeasurement();
486 ATH_MSG_WARNING(
"MdtSegmentFitter() - Unsupported measurment type" <<
typeid(measurement).
name());
490 ATH_MSG_VERBOSE(
"Update derivatives for hit "<< (hit->spacePoint() ? idHelperSvc->toString(hit->spacePoint()->identify()) :
"beamspot"));
496 for (
int k =1;
k < 5 - (!fitResult.timeFit); ++
k){
497 for (
int l = 0;
l<
k; ++
l){
498 hessian(
l,
k) = hessian(
k,
l);
503 fitResult.converged =
true;
504 ATH_MSG_VERBOSE(
"Fit converged after "<<fitResult.nIter<<
" iterations with "<<fitResult.chi2);
510 if (!fitResult.nPhiMeas && !fitResult.timeFit) {
511 paramUpdate = updateParameters<2>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
512 }
else if (!fitResult.nPhiMeas && fitResult.timeFit) {
520 paramUpdate = updateParameters<3>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
526 }
else if (fitResult.nPhiMeas && !fitResult.timeFit) {
527 paramUpdate = updateParameters<4>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
528 }
else if (fitResult.nPhiMeas && fitResult.timeFit) {
529 paramUpdate = updateParameters<5>(fitResult.segmentPars, prevPars, gradient, prevGrad, hessian);
533 fitResult.converged =
true;
544 fitResult.nDoF-=fitResult.timeFit;
547 const auto [segPos, segDir] = fitResult.makeLine();
550 std::optional<double> toF = fitResult.timeFit ? std::make_optional<double>((localToGlobal * segPos).
mag() * c_inv) : std::nullopt;
552 std::ranges::stable_sort(fitResult.calibMeasurements, [](
const HitType&
a,
const HitType&
b){
553 return a->positionInChamber().z() < b->positionInChamber().z();
558 for (
const HitType& hit : fitResult.calibMeasurements) {
559 hit->setDriftRadius(std::abs(hit->driftRadius()));
563 if (!fitResult.nPhiMeas&& !fitResult.timeFit) {
564 blockCovariance<2>(std::move(hessian), fitResult.segmentParErrs);
565 }
else if (!fitResult.nPhiMeas && fitResult.timeFit) {
568 blockCovariance<3>(std::move(hessian), fitResult.segmentParErrs);
571 }
else if (fitResult.nPhiMeas) {
572 blockCovariance<4>(std::move(hessian), fitResult.segmentParErrs);
573 }
else if (fitResult.nPhiMeas && fitResult.timeFit) {
574 blockCovariance<5>(std::move(hessian), fitResult.segmentParErrs);
582 double&
chi2,
int startPar)
const {
586 for (
int p = startPar;
p >=0 ; --
p) {
587 gradient[
p] +=2.*covRes.dot(fitMeas.
gradient[
p]);
588 for (
int k=
p;
k>=0; --
k) {
591 const int symMatIdx = vecIdxFromSymMat<ResidualWithPartials::nPars>(
p,
k);
597 <<
toString(gradient)<<
", Hessian:\n"<<hessian<<
", measurement covariance\n"<<
toString(invCov));
600 template <
unsigned int nDim>
604 covariance.setIdentity();
605 AmgSymMatrix(nDim) miniHessian = hessian.block<nDim, nDim>(0,0);
606 if (std::abs(miniHessian.determinant()) <= detCutOff) {
607 ATH_MSG_VERBOSE(
"Boeser mini hessian ("<<miniHessian.determinant()<<
")\n"<<miniHessian<<
"\n\n"<<hessian);
610 ATH_MSG_VERBOSE(
"Hessian matrix: \n"<<hessian<<
",\nblock Hessian:\n"<<miniHessian<<
",\n determinant: "<<miniHessian.determinant());
611 covariance.block<nDim,nDim>(0,0) = miniHessian.inverse();
615 template <
unsigned int nDim>
621 AmgSymMatrix(nDim) miniHessian = currentHessian.block<nDim, nDim>(0,0);
623 <<
", hessian ("<<miniHessian.determinant()<<
")"<<std::endl<<miniHessian);
625 double changeMag{0.};
626 if (std::abs(miniHessian.determinant()) > detCutOff) {
627 prevPars.block<nDim,1>(0,0) = currPars.block<nDim,1>(0,0);
629 const AmgVector(nDim) updateMe = miniHessian.inverse()* currGrad.block<nDim, 1>(0,0);
630 changeMag = std::sqrt(updateMe.dot(updateMe));
631 currPars.block<nDim,1>(0,0) -= updateMe;
632 prevGrad.block<nDim,1>(0,0) = currGrad.block<nDim,1>(0,0);
633 ATH_MSG_DEBUG(
"Hessian inverse:\n"<<miniHessian.inverse()<<
"\n\nUpdate the parameters by -"
636 const AmgVector(nDim) gradDiff = (currGrad - prevGrad).block<nDim,1>(0,0);
637 const double gradDiffMag = gradDiff.mag2();
638 double denom = (gradDiffMag > std::numeric_limits<float>::epsilon() ? gradDiffMag : 1.);
639 const double gamma = std::abs((currPars - prevPars).block<nDim,1>(0,0).
dot(gradDiff))
641 ATH_MSG_VERBOSE(
"Hessian determinant invalid. Try deepest descent - \nprev parameters: "
643 prevPars.block<nDim, 1>(0,0) = currPars.block<nDim, 1>(0,0);
644 currPars.block<nDim, 1>(0,0) -=
gamma* currGrad.block<nDim, 1>(0,0);
645 prevGrad.block<nDim, 1>(0,0) = currGrad.block<nDim,1>(0,0);
646 changeMag = std::abs(
gamma) * currGrad.block<nDim, 1>(0,0).
mag();
649 unsigned int nOutOfBound{0};
650 for (
unsigned int p = 0;
p< nDim; ++
p) {
651 double& parValue{currPars[
p]};
652 if constexpr(nDim == 3) {