159using namespace Eigen;
174 bool flagCurv,
bool flagU1dir,
bool flagU2dir) :
175 numAllPoints(aPointList.size()), numPoints(), numOffsetPoints(0), numOffsets(
176 0), numInnerTransformations(0), numInnerTransOffsets(0), numCurvature(
177 flagCurv ? 1 : 0), numParameters(0), numLocals(0), numMeasurements(
178 0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
179 0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
186 thePoints.emplace_back(std::move(aPointList));
197 const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> > &aPointsAndTransList) :
198 numAllPoints(), numPoints(), numOffsetPoints(0), numOffsets(0), numInnerTransformations(
199 aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
200 0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
201 0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
203 for (
unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
204 thePoints.push_back(aPointsAndTransList[iTraj].first);
220#ifdef GBL_EIGEN_SUPPORT_ROOT
234 unsigned int aLabel,
const TMatrixDSym &aSeed,
bool flagCurv,
235 bool flagU1dir,
bool flagU2dir) :
236numAllPoints(aPointList.size()), numPoints(), numOffsetPoints(0), numOffsets(0),
237numInnerTransformations(0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
238numLocals(0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(0),
239maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(),
240scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(),
241externalMeasurements(), externalPrecisions() {
248 unsigned int nParSeed = aSeed.GetNrows();
250 for (
unsigned int i = 0; i < nParSeed; ++i) {
251 for (
unsigned int j = 0; j < nParSeed; ++j) {
256 thePoints.emplace_back(std::move(aPointList));
267numAllPoints(), numPoints(), numOffsetPoints(0), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
268numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
269skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
270measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
271externalDerivatives(), externalMeasurements(), externalPrecisions() {
273 for (
unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
274 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
278 unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
279 unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
280 MatrixXd aTrans(nRowTrans, nColTrans);
281 for (
unsigned int i = 0; i < nRowTrans; ++i) {
282 for (
unsigned int j = 0; j < nColTrans; ++j) {
283 aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
308 const TMatrixD &extDerivatives,
const TVectorD &extMeasurements,
309 const TVectorD &extPrecisions) :
310numAllPoints(), numPoints(), numOffsetPoints(0), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
311numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
312skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
313measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
314externalDerivatives(), externalMeasurements(), externalPrecisions() {
317 unsigned int nExtMeas = extDerivatives.GetNrows();
318 unsigned int nExtPar = extDerivatives.GetNcols();
322 for (
unsigned int i = 0; i < nExtMeas; ++i) {
325 for (
unsigned int j = 0; j < nExtPar; ++j) {
329 for (
unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
330 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
334 unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
335 unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
336 MatrixXd aTrans(nRowTrans, nColTrans);
337 for (
unsigned int i = 0; i < nRowTrans; ++i) {
338 for (
unsigned int j = 0; j < nColTrans; ++j) {
339 aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
364 const TMatrixD &extDerivatives,
const TVectorD &extMeasurements,
365 const TMatrixDSym &extPrecisions) :
366numAllPoints(), numPoints(), numOffsetPoints(0), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
367numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
368skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
369measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
372 TMatrixDSymEigen extEigen(extPrecisions);
373 TMatrixD extTransformation = extEigen.GetEigenVectors();
374 extTransformation.T();
375 TMatrixD aDerivatives = extTransformation * extDerivatives;
376 TVectorD aMeasurements = extTransformation * extMeasurements;
377 TVectorD aPrecisions = extEigen.GetEigenValues();
379 unsigned int nExtMeas = aDerivatives.GetNrows();
380 unsigned int nExtPar = aDerivatives.GetNcols();
384 for (
unsigned int i = 0; i < nExtMeas; ++i) {
387 for (
unsigned int j = 0; j < nExtPar; ++j) {
391 for (
unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
392 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
396 unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
397 unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
398 MatrixXd aTrans(nRowTrans, nColTrans);
399 for (
unsigned int i = 0; i < nRowTrans; ++i) {
400 for (
unsigned int j = 0; j < nColTrans; ++j) {
401 aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
439 unsigned int aLabel = 0;
441 std::cout <<
" GblTrajectory construction failed: too few GblPoints "
447 std::vector<GblMeasurement>::const_iterator itMeas;
451 std::vector<GblPoint>::iterator itPoint;
453 itPoint <
thePoints[iTraj].end(); ++itPoint) {
454 for (itMeas = itPoint->getMeasBegin();
455 itMeas < itPoint->getMeasEnd(); ++itMeas) {
456 if (itMeas->isEnabled()) {
461 itPoint->setLabel(++aLabel);
468 }
catch (std::overflow_error &e) {
469 std::cout <<
" GblTrajectory construction failed: " << e.what()
473 std::cout <<
" GblTrajectory construction failed with exception " << i
499 std::vector<GblPoint>::iterator itPoint;
500 for (itPoint =
thePoints[iTraj].begin() + 1;
501 itPoint <
thePoints[iTraj].end() - 1; ++itPoint) {
502 unsigned int scatDim = itPoint->getScatDim();
518 if (
thePoints[iTraj].back().getScatDim() == 4)
531 unsigned int numStep = 0;
532 std::vector<GblPoint>::iterator itPoint;
533 for (itPoint =
thePoints[iTraj].begin() + 1;
534 itPoint <
thePoints[iTraj].end(); ++itPoint) {
536 scatJacobian = itPoint->getP2pJacobian();
538 scatJacobian = itPoint->getP2pJacobian() * scatJacobian;
541 itPoint->addPrevJacobian(scatJacobian);
542 if (itPoint->getOffset() >= 0) {
545 previousPoint = &(*itPoint);
549 for (itPoint =
thePoints[iTraj].end() - 1;
550 itPoint >
thePoints[iTraj].begin(); --itPoint) {
551 if (itPoint->getOffset() >= 0) {
555 itPoint->addNextJacobian(scatJacobian);
556 scatJacobian = scatJacobian * itPoint->getP2pJacobian();
571 int aSignedLabel)
const {
577 unsigned int aLabel = abs(aSignedLabel);
578 unsigned int firstLabel = 1;
579 unsigned int lastLabel = 0;
580 unsigned int aTrajectory = 0;
585 if (aLabel <= lastLabel)
592 if (aSignedLabel > 0) {
594 if (aLabel >= lastLabel) {
600 if (aLabel <= firstLabel) {
607 std::array<unsigned int, 5> labDer;
611 unsigned int nBand = 0;
615 for (
unsigned int i = 0; i < 5; ++i)
616 if (labDer[i] > lastExt)
622 unsigned int nBorder = nCurv + nLocals;
623 unsigned int nParBRL = nBorder + nBand;
624 unsigned int nParLoc = nLocals + 5;
625 std::vector<unsigned int> anIndex;
626 anIndex.reserve(nParBRL);
627 MatrixXd aJacobian(nParLoc, nParBRL);
631 for (
unsigned int i = 0; i < nLocals; ++i) {
632 aJacobian(i + 5, i) = 1.0;
633 anIndex.push_back(i + 1);
636 unsigned int iCol = nLocals;
640 unsigned int nSimple = nCurv + nBand;
641 MatrixXd aTrans(5, nSimple);
644 aTrans.block(0, 0, 1, nCurv) =
innerTransDer[aTrajectory].block(0, 0, 1,
646 for (
unsigned int i = 0; i < nCurv; ++i)
647 anIndex.push_back(++iCol);
651 aTrans.block(1, 0, 4 - nBand, nCurv) =
655 for (
unsigned int i = 5 - nBand; i < 5; ++i) {
656 aTrans(i, iCol++) = 1.;
661 aJacobian.block(0, nLocals, 5, nSimple) = matDer * aTrans;
664 for (
unsigned int i = 0; i < 5; ++i) {
666 anIndex.push_back(labDer[i]);
667 for (
unsigned int j = 0; j < 5; ++j) {
668 aJacobian(j, iCol) = matDer(j, i);
674 return std::make_pair(anIndex, aJacobian);
689 unsigned int nJacobian)
const {
702 Matrix2d prevW, prevWJ, nextW, nextWJ, matN;
703 Vector2d prevWd, nextWd;
706 const Matrix2d sumWJ(prevWJ + nextWJ);
707 matN = sumWJ.inverse();
709 const Matrix2d prevNW(matN * prevW);
710 const Matrix2d nextNW(matN * nextW);
711 const Vector2d prevNd(matN * prevWd);
712 const Vector2d nextNd(matN * nextWd);
714 unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1;
718 aJacobian.block<2, 1>(3, 0) = -prevNd - nextNd;
719 anIndex[0] = nLocals + 1;
721 aJacobian.block<2, 2>(3, 1) = prevNW;
722 aJacobian.block<2, 2>(3, 3) = nextNW;
723 for (
unsigned int i = 0; i < nDim; ++i) {
731 const Matrix2d prevWPN(nextWJ * prevNW);
732 const Matrix2d nextWPN(prevWJ * nextNW);
733 const Vector2d prevWNd(nextWJ * prevNd);
734 const Vector2d nextWNd(prevWJ * nextNd);
736 aJacobian(0, 0) = 1.0;
737 aJacobian.block<2, 1>(1, 0) = prevWNd - nextWNd;
739 aJacobian.block<2, 2>(1, 1) = -prevWPN;
740 aJacobian.block<2, 2>(1, 3) = nextWPN;
748 nOffset += nJacobian;
749 unsigned int iOff1 = nDim * nOffset + nCurv + nLocals + 1;
750 unsigned int index1 = 3 - 2 * nJacobian;
751 unsigned int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1);
752 unsigned int index2 = 1 + 2 * nJacobian;
754 aJacobian(3, index1) = 1.0;
755 aJacobian(4, index1 + 1) = 1.0;
756 for (
unsigned int i = 0; i < nDim; ++i) {
762 Matrix2d matW, matWJ;
765 double sign = (nJacobian > 0) ? 1. : -1.;
767 aJacobian(0, 0) = 1.0;
768 aJacobian.block<2, 1>(1, 0) = -sign * vecWd;
769 anIndex[0] = nLocals + 1;
771 aJacobian.block<2, 2>(1, index1) = -sign * matWJ;
772 aJacobian.block<2, 2>(1, index2) = sign * matW;
773 for (
unsigned int i = 0; i < nDim; ++i) {
789 std::array<unsigned int, 9> &anIndex,
Matrix49d &aJacobian,
799 aJacobian.topRows<2>().setZero();
801 Matrix2d prevW, prevWJ, nextW, nextWJ;
802 Vector2d prevWd, nextWd;
805 const Matrix2d sumWJ(prevWJ + nextWJ);
806 const Vector2d sumWd(prevWd + nextWd);
808 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1;
812 aJacobian.block<2, 1>(0, 0) = -sumWd;
813 anIndex[0] = nLocals + 1;
815 aJacobian.block<2, 2>(0, 1) = prevW;
816 aJacobian.block<2, 2>(0, 3) = -sumWJ;
817 aJacobian.block<2, 2>(0, 5) = nextW;
818 for (
unsigned int i = 0; i < nDim; ++i) {
835 std::array<unsigned int, 9> &anIndex,
Matrix49d &aJacobian,
845 aJacobian.leftCols<4>().setZero();
847 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1;
850 aJacobian(2, 0) = -1.;
851 aJacobian(3, 1) = -1.;
852 aJacobian(2, 2) = +1.;
853 aJacobian(3, 3) = +1.;
855 for (
unsigned int i = 0; i < nDim; ++i) {
871 std::array<unsigned int, 9> &anIndex,
Matrix49d &aJacobian,
883 Matrix2d prevW, prevWJ, nextW, nextWJ;
884 Vector2d prevWd, nextWd;
888 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1;
892 aJacobian.block<2, 1>(0, 0) = -(prevWd + nextWd);
893 anIndex[0] = nLocals + 1;
895 aJacobian.block<2, 2>(0, 1) = prevW;
896 aJacobian.block<2, 2>(0, 3) = -prevWJ;
897 aJacobian.block<2, 2>(0, 5) = -nextWJ;
898 aJacobian.block<2, 2>(0, 7) = nextW;
900 aJacobian(2, 3) = -1.;
901 aJacobian(3, 4) = -1.;
902 aJacobian(2, 5) = +1.;
903 aJacobian(3, 6) = +1.;
905 for (
unsigned int i = 0; i < nDim; ++i) {
923 Eigen::MatrixXd &extCov)
const {
929 std::vector<unsigned int> index;
930 for (
unsigned int i = 0; i < nExt; ++i) {
955 Eigen::VectorXd &localPar, Eigen::MatrixXd &localCov)
const {
958 std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
960 unsigned int nParBrl = indexAndJacobian.first.size();
961 VectorXd aVec(nParBrl);
962 for (
unsigned int i = 0; i < nParBrl; ++i) {
963 aVec[i] =
theVector(indexAndJacobian.first[i] - 1);
966 localPar = indexAndJacobian.second * aVec;
967 localCov = indexAndJacobian.second * aMat
968 * indexAndJacobian.second.transpose();
986 unsigned int &numData, Eigen::VectorXd &aResiduals,
987 Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
988 Eigen::VectorXd &aDownWeights) {
995 for (
unsigned int i = 0; i < numData; ++i) {
997 aMeasErrors(i), aResErrors(i), aDownWeights(i));
1014 unsigned int &numData, Eigen::VectorXd &aResiduals,
1015 Eigen::VectorXd &aMeasErrors) {
1022 for (
unsigned int i = 0; i < numData; ++i) {
1023 getResAndErr(firstData + i, aResiduals(i), aMeasErrors(i));
1042 unsigned int &numData, Eigen::VectorXd &aResiduals,
1043 Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
1044 Eigen::VectorXd &aDownWeights) {
1051 for (
unsigned int i = 0; i < numData; ++i) {
1052 getResAndErr(firstData + i,
true, aResiduals(i), aMeasErrors(i),
1053 aResErrors(i), aDownWeights(i));
1058#ifdef GBL_EIGEN_SUPPORT_ROOT
1068 TMatrixDSym &extCov)
const {
1073 VectorXd aVec(nExt);
1074 std::vector<unsigned int> index;
1075 for (
unsigned int i = 0; i < nExt; ++i) {
1081 unsigned int nParOut = extPar.GetNrows();
1082 for (
unsigned int i = 0; i < nParOut; ++i) {
1083 extPar[i] = aVec(i);
1084 for (
unsigned int j = 0; j < nParOut; ++j) {
1085 extCov(i, j) = aMat(i, j);
1107 TMatrixDSym &localCov)
const {
1110 std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
1112 unsigned int nParBrl = indexAndJacobian.first.size();
1113 VectorXd aVec(nParBrl);
1114 for (
unsigned int i = 0; i < nParBrl; ++i) {
1115 aVec[i] =
theVector(indexAndJacobian.first[i] - 1);
1118 VectorXd aLocalPar = indexAndJacobian.second * aVec;
1119 MatrixXd aLocalCov = indexAndJacobian.second * aMat
1120 * indexAndJacobian.second.transpose();
1122 unsigned int nParOut = localPar.GetNrows();
1123 for (
unsigned int i = 0; i < nParOut; ++i) {
1124 localPar[i] = aLocalPar(i);
1125 for (
unsigned int j = 0; j < nParOut; ++j) {
1126 localCov(i, j) = aLocalCov(i, j);
1146 unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
1147 TVectorD &aResErrors, TVectorD &aDownWeights) {
1154 for (
unsigned int i = 0; i < numData; ++i) {
1156 aMeasErrors[i], aResErrors[i], aDownWeights[i]);
1175 unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
1176 TVectorD &aResErrors, TVectorD &aDownWeights) {
1183 for (
unsigned int i = 0; i < numData; ++i) {
1184 getResAndErr(firstData + i,
true, aResiduals[i], aMeasErrors[i],
1185 aResErrors[i], aDownWeights[i]);
1198 std::vector<unsigned int> &aLabelList)
const {
1202 unsigned int aLabel = 0;
1203 unsigned int nPoint =
thePoints[0].size();
1204 aLabelList.resize(nPoint);
1205 for (
unsigned i = 0; i < nPoint; ++i) {
1206 aLabelList[i] = ++aLabel;
1217 std::vector<std::vector<unsigned int> > &aLabelList)
const {
1221 unsigned int aLabel = 0;
1224 unsigned int nPoint =
thePoints[iTraj].size();
1225 aLabelList[iTraj].resize(nPoint);
1226 for (
unsigned i = 0; i < nPoint; ++i) {
1227 aLabelList[iTraj][i] = ++aLabel;
1245 double &aResidual,
double &aMeasError,
double &aResError,
1246 double &aDownWeight) {
1249 unsigned int numLocal;
1250 unsigned int *indLocal;
1252 theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, numLocal,
1253 indLocal, derLocal);
1254 VectorXd aVec(numLocal);
1255 for (
unsigned int j = 0; j < numLocal; ++j) {
1256 aVec[j] = derLocal[j];
1259 double aFitVar = aVec.transpose() * aMat * aVec;
1260 aFitVar *= aDownWeight;
1261 aMeasError = sqrt(aMeasVar);
1263 aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.);
1265 aResError = sqrt(aMeasVar + aFitVar);
1277 double &aMeasError) {
1280 theData[aData].getResidual(aResidual, aMeasVar);
1281 aMeasError = sqrt(aMeasVar);
1291 double aValue, aWeight;
1292 unsigned int *indLocal;
1294 unsigned int numLocal;
1296 std::vector<GblData>::iterator itData;
1297 for (itData =
theData.begin(); itData <
theData.end(); ++itData) {
1303 itData->getLocalData(aValue, aWeight, numLocal, indLocal, derLocal);
1304 for (
unsigned int j = 0; j < numLocal; ++j) {
1305 theVector(indLocal[j] - 1) += derLocal[j] * aWeight * aValue;
1328 unsigned int nData = 0;
1335 if (nInnerTransRows != 5 and nInnerTransRows != 2) {
1337 <<
" GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1338 << nInnerTransRows <<
"," << nInnerTransCols
1339 <<
"): invalid number of rows" << std::endl;
1342 if (nInnerTransRows > nInnerTransCols) {
1344 <<
" GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1345 << nInnerTransRows <<
"," << nInnerTransCols
1346 <<
"): too few columns" << std::endl;
1354 <<
" GblTrajectory::prepare composed trajectory with bad inner transformation matrix["
1355 << iTraj <<
"]: different size as [0]" << std::endl;
1361 std::array<unsigned int, 5> firstLabels;
1364 if (nInnerTransRows == 5) {
1367 Matrix5d matLocalToFit = matFitToLocal.inverse();
1374 matInnerToFit.setZero();
1375 matInnerToFit(0, nInnerTransCols + iTraj) = 1.;
1376 matInnerToFit.block(1, 0, 2, nInnerTransCols) =
1385 std::vector<GblPoint>::iterator itPoint;
1386 std::vector<GblMeasurement>::const_iterator itMeas;
1390 Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor ,
1394 for (itPoint =
thePoints[iTraj].begin();
1395 itPoint <
thePoints[iTraj].end(); ++itPoint) {
1397 unsigned int nLabel = itPoint->getLabel();
1398 if (itPoint->numMeasurements()) {
1399 for (itMeas = itPoint->getMeasBegin();
1400 itMeas < itPoint->getMeasEnd(); ++itMeas) {
1402 if (!itMeas->isEnabled())
1404 unsigned int measDim = itMeas->getMeasDim();
1405 const MatrixXd localDer = itMeas->getLocalDerivatives();
1407 itMeas->getNumGlobals());
1409 itMeas->getMeasurement(matP, aMeas, aPrec);
1410 double minPrecision = itMeas->getMeasPrecMin();
1411 unsigned int iOff = 5 - measDim;
1412 std::array<unsigned int, 5> labDer;
1414 matPDer.topRows(iOff).setZero();
1416 unsigned int nJacobian =
1417 (itPoint->isLast()) ? 0 : 1;
1419 measDim, nJacobian);
1420 matPDer = matP * matDer;
1421 matPDer.bottomRows(measDim) =
1422 matP.bottomRightCorner(measDim, measDim)
1423 * matDer.bottomRows(measDim);
1427 matPDer.bottomRows<2>() = matP.bottomRightCorner<2,
1428 2>() * matDer.bottomRows<2>();
1433 proDer.resize(measDim, Eigen::NoChange);
1436 unsigned int ifirst = 0;
1438 unsigned int ilabel = 0;
1439 unsigned int numRelated = 0;
1440 while (ilabel < 5) {
1441 if (labDer[ilabel] > 0) {
1442 while (ifirst <= ilast
1444 != labDer[ilabel]) {
1447 if (ifirst > ilast) {
1449 * nDim * (iTraj + 1);
1454 for (
unsigned int k = iOff; k < 5;
1456 proDer(k - iOff, ifirst) = matPDer(
1463 if (numRelated > 0) {
1468 for (
unsigned int i = iOff; i < 5; ++i) {
1469 if (aPrec(i) > minPrecision) {
1471 aMeas(i), aPrec(i), iTraj,
1473 itMeas - itPoint->getMeasBegin());
1476 theData.emplace_back(std::move(aData));
1492 Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor ,
1498 for (itPoint =
thePoints[iTraj].begin() + 1;
1499 itPoint <
thePoints[iTraj].end(); ++itPoint) {
1500 Vector4d aMeas, aPrec;
1501 unsigned int nLabel = itPoint->getLabel();
1502 unsigned int scatDim = itPoint->getScatDim();
1505 std::array<unsigned int, 9> labDer;
1507 unsigned int numDer;
1510 if (itPoint->isLast()) {
1512 itPoint->getReducedScatterer(matT, aMeas, aPrec);
1515 matTDer.leftCols<4>() = matT * matDer.leftCols<4>();
1518 itPoint->getScatterer(matT, aMeas, aPrec);
1521 matTDer = matT * matDer;
1525 if (itPoint->isLast())
1528 itPoint->getScatterer(matT, aMeas, aPrec);
1530 matTDer.topLeftCorner<2, 7>() =
1531 matT.topLeftCorner<2, 2>()
1532 * matDer.topLeftCorner<2, 7>();
1536 proDer.resize(scatDim, Eigen::NoChange);
1539 unsigned int ifirst = 0;
1541 unsigned int ilabel = 0;
1542 unsigned int numRelated = 0;
1543 while (ilabel < numDer) {
1544 if (labDer[ilabel] > 0) {
1546 != labDer[ilabel] and ifirst <= ilast) {
1549 if (ifirst > ilast) {
1551 * nDim * (iTraj + 1);
1556 for (
unsigned int k = 0; k < scatDim; ++k) {
1557 proDer(k, ifirst) = matTDer(k, ilabel);
1563 if (numRelated > 0) {
1569 for (
unsigned int i = 0; i < scatDim; ++i) {
1570 if (aPrec(i) > 0.) {
1576 theData.emplace_back(std::move(aData));
1589 std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
1591 std::vector<unsigned int> externalSeedIndex = indexAndJacobian.first;
1592 std::vector<double> externalSeedDerivatives(externalSeedIndex.size());
1593 SelfAdjointEigenSolver<MatrixXd> externalSeedEigen(
externalSeed);
1594 VectorXd valEigen = externalSeedEigen.eigenvalues();
1595 MatrixXd vecEigen = externalSeedEigen.eigenvectors();
1596 vecEigen = vecEigen.transpose() * indexAndJacobian.second;
1598 if (valEigen(i) > 0.) {
1600 externalSeedDerivatives[j] = vecEigen(i, j);
1604 externalSeedDerivatives);
1605 theData.emplace_back(std::move(aData));
1616 if (nExtDer > nInnerTransCols) {
1618 <<
" GblTrajectory::prepare external measurement with too many derivatives: "
1619 << nExtDer <<
", defined: " << nInnerTransCols << std::endl;
1622 std::vector<unsigned int> index(nExtDer);
1623 std::vector<double> derivatives(nExtDer);
1624 for (
unsigned int iExt = 0; iExt < nExt; ++iExt) {
1625 for (
unsigned int iCol = 0; iCol < nExtDer; ++iCol) {
1632 theData.emplace_back(std::move(aData));
1641 std::vector<GblData>::iterator itData;
1642 for (itData =
theData.begin(); itData <
theData.end(); ++itData) {
1653 std::vector<GblData>::iterator itData;
1654 for (itData =
theData.begin(); itData <
theData.end(); ++itData) {
1655 aLoss += (1. - itData->setDownWeighting(aMethod));
1674 const std::string &optionList,
unsigned int aLabel) {
1675 const double normChi2[4] = { 1.0, 0.8737, 0.9326, 0.8228 };
1676 const std::string methodList =
"TtHhCc";
1684 unsigned int aMethod = 0;
1689 unsigned int ierr = 0;
1695 for (
unsigned int i = 0; i < optionList.size(); ++i)
1697 size_t aPosition = methodList.find(optionList[i]);
1698 if (aPosition != std::string::npos) {
1699 aMethod = aPosition / 2 + 1;
1708 for (
unsigned int i = 0; i <
theData.size(); ++i) {
1716 Chi2 /= normChi2[aMethod];
1720 std::cout <<
" GblTrajectory::fit exception " << e << std::endl;
1734 unsigned int aPoint;
1737 unsigned int numLocal;
1738 unsigned int *labLocal;
1740 std::vector<int> labGlobal;
1741 std::vector<double> derGlobal;
1749 std::vector<GblData>::iterator itData;
1750 for (itData =
theData.begin(); itData !=
theData.end(); ++itData) {
1751 itData->getAllData(aValue, aErr, numLocal, labLocal, derLocal, aTraj,
1752 aPoint, aMeas, aRow);
1754 thePoints[aTraj][aPoint].getGlobalLabelsAndDerivatives(aMeas, aRow,
1755 labGlobal, derGlobal);
1757 labGlobal.resize(0);
1758 aMille.
addData(aValue, aErr, numLocal, labLocal, derLocal, labGlobal,
1774 std::cout <<
"Simple GblTrajectory" << std::endl;
1777 std::cout <<
" 2D-trajectory" << std::endl;
1783 std::cout <<
" Number of (1D or 2D) offsets : " <<
numOffsets << std::endl;
1784 std::cout <<
" Number of fit parameters : " <<
numParameters
1789 std::cout <<
" Number of ext. measurements : "
1793 std::cout <<
" Label of point with ext. seed: " <<
externalPoint
1797 std::cout <<
" Constructed OK " << std::endl;
1800 std::cout <<
" Fitted OK " << std::endl;
1803 IOFormat CleanFmt(4, 0,
", ",
"\n",
"[",
"]");
1805 std::cout <<
" Inner transformations" << std::endl;
1812 std::cout <<
" External measurements" << std::endl;
1813 std::cout <<
" Measurements:" << std::endl;
1815 std::cout <<
" Precisions:" << std::endl;
1817 std::cout <<
" Derivatives:" << std::endl;
1821 std::cout <<
" External seed:" << std::endl;
1822 std::cout <<
externalSeed.format(CleanFmt) << std::endl;
1825 std::cout <<
" Fit results" << std::endl;
1826 std::cout <<
" Parameters:" << std::endl;
1828 std::cout <<
" Covariance matrix (bordered band part):"
1840 std::cout <<
"GblPoints " << std::endl;
1842 std::vector<GblPoint>::const_iterator itPoint;
1843 for (itPoint =
thePoints[iTraj].begin();
1844 itPoint <
thePoints[iTraj].end(); ++itPoint) {
1845 itPoint->printPoint(level);
1852 std::cout <<
"GblData blocks " << std::endl;
1853 std::vector<GblData>::const_iterator itData;
1854 for (itData =
theData.begin(); itData <
theData.end(); ++itData) {
1855 itData->printData();
GblTrajectory definition.
double getBandCondition() const
Get condition from band (decomposition)
void printMatrix() const
Print bordered band matrix.
Eigen::MatrixXd getBlockMatrix(const std::vector< unsigned int > anIndex) const
Retrieve symmetric block matrix.
void setZero()
Set content to 0.
void solveAndInvertBorderedBand(const VVector &aRightHandSide, VVector &aSolution)
Solve linear equation system, partially calculate inverse.
void resize(unsigned int nSize, unsigned int nBorder=1, unsigned int nBand=7)
Resize bordered band matrix.
void addBlockMatrix(double aWeight, const std::vector< unsigned int > *anIndex, const std::vector< double > *aVector)
Add symmetric block matrix.
Data (block) for independent scalar measurement.
void addDerivatives(unsigned int iRow, const std::array< unsigned int, 5 > &labDer, const Matrix5d &matDer, unsigned int iOff, const Eigen::MatrixBase< LocalDerivative > &derLocal, unsigned int extOff, const Eigen::MatrixBase< ExtDerivative > &extDer)
Add derivatives from measurement.
void getDerivatives(int aDirection, Eigen::Matrix2d &matW, Eigen::Matrix2d &matWJ, Eigen::Vector2d &vecWd) const
Retrieve derivatives of local track model.
void addNextJacobian(const Matrix5d &aJac)
Define jacobian to next scatterer (by GBLTrajectory constructor)
unsigned int getScatDim() const
Get scatterer dimension.
const Matrix5d & getP2pJacobian() const
Retrieve point-to-(previous)point jacobian.
int getOffset() const
Retrieve offset for point.
std::vector< unsigned int > numPoints
Number of points on (sub)trajectory.
std::vector< unsigned int > scatDataIndex
mapping points to data blocks from scatterers
BorderedBandMatrix theMatrix
(Bordered band) matrix of linear equation system
std::vector< std::vector< GblPoint > > thePoints
(list of) List of points on trajectory
void printPoints(unsigned int level=0) const
Print GblPoints on trajectory.
unsigned int skippedMeasLabel
Label of point with measurements skipped in fit (for unbiased residuals) (or 0)
unsigned int numTrajectories
Number of trajectories (in composed trajectory)
unsigned int getFitToStepJacobian(std::array< unsigned int, 9 > &anIndex, Matrix49d &aJacobian, const GblPoint &aPoint) const
Get jacobian for transformation from (trajectory) fit to step parameters at point.
void construct()
Construct trajectory from list of points.
unsigned int externalPoint
Label of external point (or 0)
Eigen::MatrixXd externalSeed
Precision (inverse covariance matrix) of external seed.
unsigned int getMeasResults(unsigned int aLabel, unsigned int &numData, Eigen::VectorXd &aResiduals, Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors, Eigen::VectorXd &aDownWeights)
Get residuals from fit at point for measurement "long list".
unsigned int numAllPoints
Number of all points on trajectory.
double downWeight(unsigned int aMethod)
Down-weight all points.
VVector theVector
Vector of linear equation system.
void calcJacobians()
Calculate Jacobians to previous/next scatterer from point to point ones.
unsigned int numCurvature
Number of curvature parameters (0 or 1) or external parameters.
Eigen::VectorXd externalPrecisions
Precisions for external measurements of composed trajectory.
std::vector< std::array< unsigned int, 5 > > innerTransLab
Labels at innermost points of composed trajectory.
Eigen::VectorXd externalMeasurements
Residuals for external measurements of composed trajectory.
void printTrajectory(unsigned int level=0) const
Print GblTrajectory.
void predict()
Calculate predictions for all points.
unsigned int getFitToKinkAndStepJacobian(std::array< unsigned int, 9 > &anIndex, Matrix49d &aJacobian, const GblPoint &aPoint) const
Get jacobian for transformation from (trajectory) fit to kink and step parameters at point.
void defineOffsets()
Define offsets from list of points.
std::vector< Eigen::MatrixXd > innerTransformations
Transformations at innermost points of composed trajectory (from common external parameters)
unsigned int numLocals
Total number of (additional) local parameters.
unsigned int numOffsetPoints
Number of points with offsets on trajectory.
void milleOut(MilleBinary &aMille)
Write valid trajectory to Millepede-II binary file.
unsigned int getExtResults(Eigen::VectorXd &extPar, Eigen::MatrixXd &extCov) const
Get fit results for external parameters.
void getFitToLocalJacobian(std::array< unsigned int, 5 > &anIndex, Matrix5d &aJacobian, const GblPoint &aPoint, unsigned int measDim, unsigned int nJacobian=1) const
Get (part of) jacobian for transformation from (trajectory) fit to track parameters at point.
unsigned int numInnerTransformations
Number of inner transformations to external parameters.
unsigned int maxNumGlobals
Max. number of global labels/derivatives per point.
bool isValid() const
Retrieve validity of trajectory.
double getBandCondition() const
Get condition from band (decomposition)
GblTrajectory(const std::vector< GblPoint > &aPointList, bool flagCurv=true, bool flagU1dir=true, bool flagU2dir=true)
Create new (simple) trajectory from list of points.
void prepare()
Prepare fit for simple or composed trajectory.
unsigned int getResults(int aSignedLabel, Eigen::VectorXd &localPar, Eigen::MatrixXd &localCov) const
Get fit results at point.
Eigen::MatrixXd externalDerivatives
Derivatives for external measurements of composed trajectory.
unsigned int fit(double &Chi2, int &Ndf, double &lostWeight, const std::string &optionList="", unsigned int aLabel=0)
Perform fit of (valid) trajectory.
unsigned int getScatResults(unsigned int aLabel, unsigned int &numData, Eigen::VectorXd &aResiduals, Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors, Eigen::VectorXd &aDownWeights)
Get (kink) residuals from fit at point for scatterer.
std::vector< GblData > theData
List of data blocks.
bool constructOK
Trajectory has been successfully constructed (ready for fit/output)
unsigned int numInnerTransOffsets
Number of (points with) offsets affected by inner transformations to external parameters.
std::vector< unsigned int > measDataIndex
mapping points to data blocks from measurements
void buildLinearEquationSystem()
Build linear equation system from data (blocks).
void printData() const
Print GblData blocks for trajectory.
unsigned int getLabels(std::vector< unsigned int > &aLabelList) const
Get (list of) labels of points on (simple) valid trajectory.
unsigned int numParameters
Number of fit parameters.
unsigned int numMeasurements
Total number of measurements.
std::vector< Eigen::MatrixXd > innerTransDer
Derivatives at innermost points of composed trajectory.
unsigned int getFitToKinkJacobian(std::array< unsigned int, 9 > &anIndex, Matrix49d &aJacobian, const GblPoint &aPoint) const
Get jacobian for transformation from (trajectory) fit to kink parameters at point.
std::pair< std::vector< unsigned int >, Eigen::MatrixXd > getJacobian(int aSignedLabel) const
Get jacobian for transformation from fit to track parameters at point.
void getResAndErr(unsigned int aData, bool used, double &aResidual, double &aMeasError, double &aResError, double &aDownWeight)
Get residual and errors from data block "long list".
bool fitOK
Trajectory has been successfully fitted (results are valid)
unsigned int numOffsets
Number of (1D or 2D) offsets on trajectory.
std::vector< unsigned int > theDimension
List of active dimensions (0=u1, 1=u2) in fit.
unsigned int getNumPoints() const
Retrieve number of point from trajectory.
Millepede-II (binary) record.
void writeRecord()
Write record to file.
void addData(double aMeas, double aErr, unsigned int numLocal, unsigned int *indLocal, double *derLocal, const std::vector< int > &labGlobal, const std::vector< double > &derGlobal)
Add data block to (end of) record.
void setZero()
Set content to 0.
void resize(const unsigned int nRows)
Resize vector.
void print() const
Print vector.
Namespace for the general broken lines package.
Eigen::Matrix< double, 5, 1 > Vector5d
Eigen::Matrix< double, 4, 9 > Matrix49d
Eigen::Matrix< double, 5, 5 > Matrix5d