GeneralBrokenLines V03-01-03
using EIGEN
GblTrajectory.cpp
Go to the documentation of this file.
1/*
2 * GblTrajectory.cpp
3 *
4 * Created on: Aug 18, 2011
5 * Author: kleinwrt
6 */
7
158#include "GblTrajectory.h"
159using namespace Eigen;
160
162namespace gbl {
163
165
173GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
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() {
180
181 if (flagU1dir)
182 theDimension.push_back(0);
183 if (flagU2dir)
184 theDimension.push_back(1);
185 // simple (single) trajectory
186 thePoints.emplace_back(std::move(aPointList));
187 numPoints.push_back(numAllPoints);
188 construct(); // construct trajectory
189}
190
192
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() {
202
203 for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
204 thePoints.push_back(aPointsAndTransList[iTraj].first);
205 numPoints.push_back(thePoints.back().size());
206 numAllPoints += numPoints.back();
207 innerTransformations.push_back(aPointsAndTransList[iTraj].second);
208 }
209 theDimension.push_back(0);
210 theDimension.push_back(1);
211 // kinematic (2) or geometric (1) constraint
212 numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
214 innerTransformations[0].rows() == 5 ?
215 innerTransformations[0].cols() :
217 construct(); // construct (composed) trajectory
218}
219
220#ifdef GBL_EIGEN_SUPPORT_ROOT
222
233GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
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() {
242
243 if (flagU1dir)
244 theDimension.push_back(0);
245 if (flagU2dir)
246 theDimension.push_back(1);
247 // convert from ROOT
248 unsigned int nParSeed = aSeed.GetNrows();
249 externalSeed.resize(nParSeed, nParSeed);
250 for (unsigned int i = 0; i < nParSeed; ++i) {
251 for (unsigned int j = 0; j < nParSeed; ++j) {
252 externalSeed(i, j) = aSeed(i, j);
253 }
254 }
255 // simple (single) trajectory
256 thePoints.emplace_back(std::move(aPointList));
257 numPoints.push_back(numAllPoints);
258 construct();// construct trajectory
259}
260
262
266GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList) :
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() {
272
273 for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
274 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
275 numPoints.push_back(thePoints.back().size());
276 numAllPoints += numPoints.back();
277 // convert from ROOT
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);
284 }
285 }
286 innerTransformations.emplace_back(std::move(aTrans));
287 }
288 theDimension.push_back(0);
289 theDimension.push_back(1);
290 // kinematic (2) or geometric (1) constraint
291 numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
293 innerTransformations[0].rows() == 5 ?
294 innerTransformations[0].cols() :
296 construct();// construct (composed) trajectory
297}
298
300
307GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
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() {
315
316 // convert from ROOT
317 unsigned int nExtMeas = extDerivatives.GetNrows();
318 unsigned int nExtPar = extDerivatives.GetNcols();
319 externalDerivatives.resize(nExtMeas, nExtPar);
320 externalMeasurements.resize(nExtMeas);
321 externalPrecisions.resize(nExtMeas);
322 for (unsigned int i = 0; i < nExtMeas; ++i) {
323 externalMeasurements(i) = extMeasurements[i];
324 externalPrecisions(i) = extPrecisions[i];
325 for (unsigned int j = 0; j < nExtPar; ++j) {
326 externalDerivatives(i, j) = extDerivatives(i, j);
327 }
328 }
329 for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
330 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
331 numPoints.push_back(thePoints.back().size());
332 numAllPoints += numPoints.back();
333 // convert from ROOT
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);
340 }
341 }
342 innerTransformations.emplace_back(std::move(aTrans));
343 }
344 theDimension.push_back(0);
345 theDimension.push_back(1);
346 // kinematic (2) or geometric (1) constraint
347 numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
349 innerTransformations[0].rows() == 5 ?
350 innerTransformations[0].cols() :
352 construct();// construct (composed) trajectory
353}
354
356
363GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
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() {
370
371 // diagonalize external measurement
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();
378 // convert from ROOT
379 unsigned int nExtMeas = aDerivatives.GetNrows();
380 unsigned int nExtPar = aDerivatives.GetNcols();
381 externalDerivatives.resize(nExtMeas, nExtPar);
382 externalMeasurements.resize(nExtMeas);
383 externalPrecisions.resize(nExtMeas);
384 for (unsigned int i = 0; i < nExtMeas; ++i) {
385 externalMeasurements(i) = aMeasurements[i];
386 externalPrecisions(i) = aPrecisions[i];
387 for (unsigned int j = 0; j < nExtPar; ++j) {
388 externalDerivatives(i, j) = aDerivatives(i, j);
389 }
390 }
391 for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
392 thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
393 numPoints.push_back(thePoints.back().size());
394 numAllPoints += numPoints.back();
395 // convert from ROOT
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);
402 }
403 }
404 innerTransformations.emplace_back(std::move(aTrans));
405 }
406 theDimension.push_back(0);
407 theDimension.push_back(1);
408 // kinematic (2) or geometric (1) constraint
409 numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
411 innerTransformations[0].rows() == 5 ?
412 innerTransformations[0].cols() :
414 construct();// construct (composed) trajectory
415}
416#endif
417
419}
420
423 return constructOK;
424}
425
427unsigned int GblTrajectory::getNumPoints() const {
428 return numAllPoints;
429}
430
432
436
437 constructOK = false;
438 fitOK = false;
439 unsigned int aLabel = 0;
440 if (numAllPoints < 2) {
441 std::cout << " GblTrajectory construction failed: too few GblPoints "
442 << std::endl;
443 return;
444 }
445 // loop over trajectories
446 numTrajectories = thePoints.size();
447 std::vector<GblMeasurement>::const_iterator itMeas;
448
449 //std::cout << " numTrajectories: " << numTrajectories << ", " << innerTransformations.size() << std::endl;
450 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
451 std::vector<GblPoint>::iterator itPoint;
452 for (itPoint = thePoints[iTraj].begin();
453 itPoint < thePoints[iTraj].end(); ++itPoint) {
454 for (itMeas = itPoint->getMeasBegin();
455 itMeas < itPoint->getMeasEnd(); ++itMeas) {
456 if (itMeas->isEnabled()) {
457 numLocals = std::max(numLocals, itMeas->getNumLocals());
458 numMeasurements += itMeas->getMeasDim();
459 }
460 }
461 itPoint->setLabel(++aLabel);
462 }
463 }
466 try {
467 prepare();
468 } catch (std::overflow_error &e) {
469 std::cout << " GblTrajectory construction failed: " << e.what()
470 << std::endl;
471 return;
472 } catch (int i) {
473 std::cout << " GblTrajectory construction failed with exception " << i
474 << std::endl;
475 return;
476 }
477
478 constructOK = true;
479 // number of fit parameters
483}
484
486
491
492 // loop over trajectories
493 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
494 // first point is offset
495 thePoints[iTraj].front().setType(-1);
496 thePoints[iTraj].front().setOffset(numOffsets++);
498 // intermediate scatterers are offsets
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();
503 if (scatDim) {
504 itPoint->setOffset(numOffsets++);
506 // thick scatterer?
507 if (scatDim == 4)
508 numOffsets++;
509 } else {
510 itPoint->setOffset(-numOffsets);
511 }
512 }
513 // last point is offset
514 thePoints[iTraj].back().setType(1);
515 thePoints[iTraj].back().setOffset(numOffsets++);
517 // thick scatterer at last point?
518 if (thePoints[iTraj].back().getScatDim() == 4)
519 numOffsets++;
520 }
521}
522
525
526 Matrix5d scatJacobian;
527 // loop over trajectories
528 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
529 // forward propagation (all)
530 GblPoint *previousPoint = &thePoints[iTraj].front();
531 unsigned int numStep = 0;
532 std::vector<GblPoint>::iterator itPoint;
533 for (itPoint = thePoints[iTraj].begin() + 1;
534 itPoint < thePoints[iTraj].end(); ++itPoint) {
535 if (numStep == 0) {
536 scatJacobian = itPoint->getP2pJacobian();
537 } else {
538 scatJacobian = itPoint->getP2pJacobian() * scatJacobian;
539 }
540 numStep++;
541 itPoint->addPrevJacobian(scatJacobian); // iPoint -> previous scatterer
542 if (itPoint->getOffset() >= 0) {
543 previousPoint->addNextJacobian(scatJacobian); // lastPoint -> next scatterer
544 numStep = 0;
545 previousPoint = &(*itPoint);
546 }
547 }
548 // backward propagation (without scatterers)
549 for (itPoint = thePoints[iTraj].end() - 1;
550 itPoint > thePoints[iTraj].begin(); --itPoint) {
551 if (itPoint->getOffset() >= 0) {
552 scatJacobian = itPoint->getP2pJacobian();
553 continue; // skip offsets
554 }
555 itPoint->addNextJacobian(scatJacobian); // iPoint -> next scatterer
556 scatJacobian = scatJacobian * itPoint->getP2pJacobian();
557 }
558 }
559}
560
562
570std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
571 int aSignedLabel) const {
572
573 unsigned int nDim = theDimension.size();
574 unsigned int nCurv = numCurvature;
575 unsigned int nLocals = numLocals;
576 // find trajectory, position in trajectory
577 unsigned int aLabel = abs(aSignedLabel);
578 unsigned int firstLabel = 1;
579 unsigned int lastLabel = 0;
580 unsigned int aTrajectory = 0;
581 // loop over trajectories
582 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
583 aTrajectory = iTraj;
584 lastLabel += numPoints[iTraj];
585 if (aLabel <= lastLabel)
586 break;
587 if (iTraj < numTrajectories - 1)
588 firstLabel += numPoints[iTraj];
589 }
590 int nJacobian; // 0: prev, 1: next
591 // check consistency of (index, direction)
592 if (aSignedLabel > 0) {
593 nJacobian = 1;
594 if (aLabel >= lastLabel) {
595 aLabel = lastLabel;
596 nJacobian = 0;
597 }
598 } else {
599 nJacobian = 0;
600 if (aLabel <= firstLabel) {
601 aLabel = firstLabel;
602 nJacobian = 1;
603 }
604 }
605
606 const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
607 std::array<unsigned int, 5> labDer;
608 Matrix5d matDer;
609 getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);
610
611 unsigned int nBand = 0; // number of parameters from band part
612 if (numInnerTransformations > 0) {
613 unsigned int lastExt = innerTransLab[aTrajectory][2
614 * numInnerTransOffsets]; // last label for external parameters
615 for (unsigned int i = 0; i < 5; ++i)
616 if (labDer[i] > lastExt)
617 nBand++;
618 } else {
619 nBand = 2 * nDim;
620 }
621
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);
628 aJacobian.setZero();
629
630 // from local parameters
631 for (unsigned int i = 0; i < nLocals; ++i) {
632 aJacobian(i + 5, i) = 1.0;
633 anIndex.push_back(i + 1);
634 }
635 // from trajectory parameters
636 unsigned int iCol = nLocals;
637 // composed trajectory ?
638 if (numInnerTransformations > 0) {
639 // transformation external to (simple) broken line (fit) parameters
640 unsigned int nSimple = nCurv + nBand;
641 MatrixXd aTrans(5, nSimple);
642 aTrans.setZero();
643 // external part, curvature
644 aTrans.block(0, 0, 1, nCurv) = innerTransDer[aTrajectory].block(0, 0, 1,
645 nCurv);
646 for (unsigned int i = 0; i < nCurv; ++i)
647 anIndex.push_back(++iCol);
648 if (nBand < 4) {
649 // external part, offsets (nBand=0: 2, nBand=2: 1)
650 unsigned int iRow = 1 + 2 * numInnerTransOffsets + nBand - 4; // start row (1: 1st, 3: 2nd offset)
651 aTrans.block(1, 0, 4 - nBand, nCurv) =
652 innerTransDer[aTrajectory].block(iRow, 0, 4 - nBand, nCurv);
653 }
654 // (remaining) band part
655 for (unsigned int i = 5 - nBand; i < 5; ++i) {
656 aTrans(i, iCol++) = 1.;
657 anIndex.push_back(
658 labDer[i]
659 - numInnerTransOffsets * nDim * (aTrajectory + 1)); // adjust label
660 }
661 aJacobian.block(0, nLocals, 5, nSimple) = matDer * aTrans;
662 } else {
663 // simple trajectory
664 for (unsigned int i = 0; i < 5; ++i) {
665 if (labDer[i] > 0) {
666 anIndex.push_back(labDer[i]);
667 for (unsigned int j = 0; j < 5; ++j) {
668 aJacobian(j, iCol) = matDer(j, i);
669 }
670 ++iCol;
671 }
672 }
673 }
674 return std::make_pair(anIndex, aJacobian);
675}
676
678
687void GblTrajectory::getFitToLocalJacobian(std::array<unsigned int, 5> &anIndex,
688 Matrix5d &aJacobian, const GblPoint &aPoint, unsigned int measDim,
689 unsigned int nJacobian) const {
690
691 unsigned int nDim = theDimension.size();
692 unsigned int nCurv = numCurvature;
693 unsigned int nLocals = numLocals;
694
695 int nOffset = aPoint.getOffset();
696 unsigned int scatDim = aPoint.getScatDim();
697
698 anIndex = { }; // reset to 0
699 aJacobian.setZero();
700 if (nOffset < 0) // need interpolation
701 {
702 Matrix2d prevW, prevWJ, nextW, nextWJ, matN;
703 Vector2d prevWd, nextWd;
704 aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
705 aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W+, W+ * J+, W+ * d+
706 const Matrix2d sumWJ(prevWJ + nextWJ);
707 matN = sumWJ.inverse(); // N = (W- * J- + W+ * J+)^-1
708 // derivatives for u_int
709 const Matrix2d prevNW(matN * prevW); // N * W-
710 const Matrix2d nextNW(matN * nextW); // N * W+
711 const Vector2d prevNd(matN * prevWd); // N * W- * d-
712 const Vector2d nextNd(matN * nextWd); // N * W+ * d+
713
714 unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i)
715
716 // local offset
717 if (nCurv > 0) {
718 aJacobian.block<2, 1>(3, 0) = -prevNd - nextNd; // from curvature
719 anIndex[0] = nLocals + 1;
720 }
721 aJacobian.block<2, 2>(3, 1) = prevNW; // from 1st Offset
722 aJacobian.block<2, 2>(3, 3) = nextNW; // from 2nd Offset
723 for (unsigned int i = 0; i < nDim; ++i) {
724 anIndex[1 + theDimension[i]] = iOff + i;
725 anIndex[3 + theDimension[i]] = iOff + nDim + i;
726 }
727
728 // local slope and curvature
729 if (measDim > 2) {
730 // derivatives for u'_int
731 const Matrix2d prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
732 const Matrix2d nextWPN(prevWJ * nextNW); // W- * J- * N * W+
733 const Vector2d prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
734 const Vector2d nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
735 if (nCurv > 0) {
736 aJacobian(0, 0) = 1.0;
737 aJacobian.block<2, 1>(1, 0) = prevWNd - nextWNd; // from curvature
738 }
739 aJacobian.block<2, 2>(1, 1) = -prevWPN; // from 1st Offset
740 aJacobian.block<2, 2>(1, 3) = nextWPN; // from 2nd Offset
741 }
742 } else { // at point
743 // anIndex must be sorted
744 // forward : iOff2 = iOff1 + nDim, index1 = 1, index2 = 3
745 // backward: iOff2 = iOff1 - nDim, index1 = 3, index2 = 1
746 // adjust for thick scatterer (before/after)
747 if (scatDim == 4)
748 nOffset += nJacobian;
749 unsigned int iOff1 = nDim * nOffset + nCurv + nLocals + 1; // first offset ('i' in u_i)
750 unsigned int index1 = 3 - 2 * nJacobian; // index of first offset
751 unsigned int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1); // second offset ('i' in u_i)
752 unsigned int index2 = 1 + 2 * nJacobian; // index of second offset
753 // local offset
754 aJacobian(3, index1) = 1.0; // from 1st Offset
755 aJacobian(4, index1 + 1) = 1.0;
756 for (unsigned int i = 0; i < nDim; ++i) {
757 anIndex[index1 + theDimension[i]] = iOff1 + i;
758 }
759
760 // local slope and curvature
761 if (measDim > 2) {
762 Matrix2d matW, matWJ;
763 Vector2d vecWd;
764 aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d
765 double sign = (nJacobian > 0) ? 1. : -1.;
766 if (nCurv > 0) {
767 aJacobian(0, 0) = 1.0;
768 aJacobian.block<2, 1>(1, 0) = -sign * vecWd; // from curvature
769 anIndex[0] = nLocals + 1;
770 }
771 aJacobian.block<2, 2>(1, index1) = -sign * matWJ; // from 1st Offset
772 aJacobian.block<2, 2>(1, index2) = sign * matW; // from 2nd Offset
773 for (unsigned int i = 0; i < nDim; ++i) {
774 anIndex[index2 + theDimension[i]] = iOff2 + i;
775 }
776 }
777 }
778}
779
781
789 std::array<unsigned int, 9> &anIndex, Matrix49d &aJacobian,
790 const GblPoint &aPoint) const {
791
792 unsigned int nDim = theDimension.size();
793 unsigned int nCurv = numCurvature;
794 unsigned int nLocals = numLocals;
795
796 int nOffset = aPoint.getOffset();
797
798 anIndex = { }; // reset to 0
799 aJacobian.topRows<2>().setZero();
800
801 Matrix2d prevW, prevWJ, nextW, nextWJ;
802 Vector2d prevWd, nextWd;
803 aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
804 aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
805 const Matrix2d sumWJ(prevWJ + nextWJ); // W- * J- + W+ * J+
806 const Vector2d sumWd(prevWd + nextWd); // W+ * d+ + W- * d-
807
808 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)
809
810 // local offset
811 if (nCurv > 0) {
812 aJacobian.block<2, 1>(0, 0) = -sumWd; // from curvature
813 anIndex[0] = nLocals + 1;
814 }
815 aJacobian.block<2, 2>(0, 1) = prevW; // from 1st Offset
816 aJacobian.block<2, 2>(0, 3) = -sumWJ; // from 2nd Offset
817 aJacobian.block<2, 2>(0, 5) = nextW; // from 3rd Offset
818 for (unsigned int i = 0; i < nDim; ++i) {
819 anIndex[1 + theDimension[i]] = iOff + i;
820 anIndex[3 + theDimension[i]] = iOff + nDim + i;
821 anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i;
822 }
823 return 7;
824}
825
827
835 std::array<unsigned int, 9> &anIndex, Matrix49d &aJacobian,
836 const GblPoint &aPoint) const {
837
838 unsigned int nDim = theDimension.size();
839 unsigned int nCurv = numCurvature;
840 unsigned int nLocals = numLocals;
841
842 int nOffset = aPoint.getOffset();
843
844 anIndex = { }; // reset to 0
845 aJacobian.leftCols<4>().setZero();
846
847 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)
848
849 // step
850 aJacobian(2, 0) = -1.; // from 2nd Offset
851 aJacobian(3, 1) = -1.; // from 2nd Offset
852 aJacobian(2, 2) = +1.; // from 3rd Offset
853 aJacobian(3, 3) = +1.; // from 3rd Offset
854
855 for (unsigned int i = 0; i < nDim; ++i) {
856 anIndex[0 + theDimension[i]] = iOff + nDim + i;
857 anIndex[2 + theDimension[i]] = iOff + nDim * 2 + i;
858 }
859 return 4;
860}
861
863
871 std::array<unsigned int, 9> &anIndex, Matrix49d &aJacobian,
872 const GblPoint &aPoint) const {
873
874 unsigned int nDim = theDimension.size();
875 unsigned int nCurv = numCurvature;
876 unsigned int nLocals = numLocals;
877
878 int nOffset = aPoint.getOffset();
879
880 anIndex = { }; // reset to 0
881 aJacobian.setZero();
882
883 Matrix2d prevW, prevWJ, nextW, nextWJ;
884 Vector2d prevWd, nextWd;
885 aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
886 aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
887
888 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)
889
890 // kink
891 if (nCurv > 0) {
892 aJacobian.block<2, 1>(0, 0) = -(prevWd + nextWd); // from curvature
893 anIndex[0] = nLocals + 1;
894 }
895 aJacobian.block<2, 2>(0, 1) = prevW; // from 1st Offset
896 aJacobian.block<2, 2>(0, 3) = -prevWJ; // from 2nd Offset
897 aJacobian.block<2, 2>(0, 5) = -nextWJ; // from 3rd Offset
898 aJacobian.block<2, 2>(0, 7) = nextW; // from 4th Offset
899 // step
900 aJacobian(2, 3) = -1.; // from 2nd Offset
901 aJacobian(3, 4) = -1.; // from 2nd Offset
902 aJacobian(2, 5) = +1.; // from 3rd Offset
903 aJacobian(3, 6) = +1.; // from 3rd Offset
904
905 for (unsigned int i = 0; i < nDim; ++i) {
906 anIndex[1 + theDimension[i]] = iOff + i;
907 anIndex[3 + theDimension[i]] = iOff + nDim + i;
908 anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i;
909 anIndex[7 + theDimension[i]] = iOff + nDim * 3 + i;
910 }
911 return 9;
912}
913
915
922unsigned int GblTrajectory::getExtResults(Eigen::VectorXd &extPar,
923 Eigen::MatrixXd &extCov) const {
924 if (not fitOK)
925 return 1;
926 // get external parameters (single block after locals)
927 unsigned int nExt = innerTransformations[0].cols();
928 VectorXd aVec(nExt); // compressed vector
929 std::vector<unsigned int> index;
930 for (unsigned int i = 0; i < nExt; ++i) {
931 aVec[i] = theVector(numLocals + i);
932 index.push_back(numLocals + i + 1);
933 }
934 extPar = aVec;
935 extCov = theMatrix.getBlockMatrix(index); // compressed matrix
936 return 0;
937}
938
940
954unsigned int GblTrajectory::getResults(int aSignedLabel,
955 Eigen::VectorXd &localPar, Eigen::MatrixXd &localCov) const {
956 if (not fitOK)
957 return 1;
958 std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
959 getJacobian(aSignedLabel);
960 unsigned int nParBrl = indexAndJacobian.first.size();
961 VectorXd aVec(nParBrl); // compressed vector
962 for (unsigned int i = 0; i < nParBrl; ++i) {
963 aVec[i] = theVector(indexAndJacobian.first[i] - 1);
964 }
965 MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
966 localPar = indexAndJacobian.second * aVec;
967 localCov = indexAndJacobian.second * aMat
968 * indexAndJacobian.second.transpose();
969 return 0;
970}
971
973
985unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
986 unsigned int &numData, Eigen::VectorXd &aResiduals,
987 Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
988 Eigen::VectorXd &aDownWeights) {
989 numData = 0;
990 if (not fitOK)
991 return 1;
992
993 unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
994 numData = measDataIndex[aLabel] - firstData; // number of data blocks
995 for (unsigned int i = 0; i < numData; ++i) {
996 getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals(i),
997 aMeasErrors(i), aResErrors(i), aDownWeights(i));
998 }
999 return 0;
1000}
1001
1003
1013unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
1014 unsigned int &numData, Eigen::VectorXd &aResiduals,
1015 Eigen::VectorXd &aMeasErrors) {
1016 numData = 0;
1017 if (not fitOK)
1018 return 1;
1019
1020 unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
1021 numData = measDataIndex[aLabel] - firstData; // number of data blocks
1022 for (unsigned int i = 0; i < numData; ++i) {
1023 getResAndErr(firstData + i, aResiduals(i), aMeasErrors(i));
1024 }
1025 return 0;
1026}
1027
1029
1041unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
1042 unsigned int &numData, Eigen::VectorXd &aResiduals,
1043 Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
1044 Eigen::VectorXd &aDownWeights) {
1045 numData = 0;
1046 if (not fitOK)
1047 return 1;
1048
1049 unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
1050 numData = scatDataIndex[aLabel] - firstData; // number of data blocks
1051 for (unsigned int i = 0; i < numData; ++i) {
1052 getResAndErr(firstData + i, true, aResiduals(i), aMeasErrors(i),
1053 aResErrors(i), aDownWeights(i));
1054 }
1055 return 0;
1056}
1057
1058#ifdef GBL_EIGEN_SUPPORT_ROOT
1060
1067unsigned int GblTrajectory::getExtResults(TVectorD &extPar,
1068 TMatrixDSym &extCov) const {
1069 if (not fitOK)
1070 return 1;
1071 // get external parameters (single block after locals)
1072 unsigned int nExt = innerTransformations[0].cols();
1073 VectorXd aVec(nExt); // compressed vector
1074 std::vector<unsigned int> index;
1075 for (unsigned int i = 0; i < nExt; ++i) {
1076 aVec[i] = theVector(numLocals + i);
1077 index.push_back(numLocals + i + 1);
1078 }
1079 MatrixXd aMat = theMatrix.getBlockMatrix(index); // compressed matrix
1080 // convert to ROOT
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);
1086 }
1087 }
1088 return 0;
1089}
1090
1092
1106unsigned int GblTrajectory::getResults(int aSignedLabel, TVectorD &localPar,
1107 TMatrixDSym &localCov) const {
1108 if (not fitOK)
1109 return 1;
1110 std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
1111 getJacobian(aSignedLabel);
1112 unsigned int nParBrl = indexAndJacobian.first.size();
1113 VectorXd aVec(nParBrl); // compressed vector
1114 for (unsigned int i = 0; i < nParBrl; ++i) {
1115 aVec[i] = theVector(indexAndJacobian.first[i] - 1);
1116 }
1117 MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
1118 VectorXd aLocalPar = indexAndJacobian.second * aVec;
1119 MatrixXd aLocalCov = indexAndJacobian.second * aMat
1120 * indexAndJacobian.second.transpose();
1121 // convert to ROOT
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);
1127 }
1128 }
1129 return 0;
1130}
1131
1133
1145unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
1146 unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
1147 TVectorD &aResErrors, TVectorD &aDownWeights) {
1148 numData = 0;
1149 if (not fitOK)
1150 return 1;
1151
1152 unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
1153 numData = measDataIndex[aLabel] - firstData;// number of data blocks
1154 for (unsigned int i = 0; i < numData; ++i) {
1155 getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals[i],
1156 aMeasErrors[i], aResErrors[i], aDownWeights[i]);
1157 }
1158 return 0;
1159}
1160
1162
1174unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
1175 unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
1176 TVectorD &aResErrors, TVectorD &aDownWeights) {
1177 numData = 0;
1178 if (not fitOK)
1179 return 1;
1180
1181 unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
1182 numData = scatDataIndex[aLabel] - firstData;// number of data blocks
1183 for (unsigned int i = 0; i < numData; ++i) {
1184 getResAndErr(firstData + i, true, aResiduals[i], aMeasErrors[i],
1185 aResErrors[i], aDownWeights[i]);
1186 }
1187 return 0;
1188}
1189
1190#endif
1191
1193
1198 std::vector<unsigned int> &aLabelList) const {
1199 if (not constructOK)
1200 return 1;
1201
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;
1207 }
1208 return 0;
1209}
1210
1212
1217 std::vector<std::vector<unsigned int> > &aLabelList) const {
1218 if (not constructOK)
1219 return 1;
1220
1221 unsigned int aLabel = 0;
1222 aLabelList.resize(numTrajectories);
1223 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
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;
1228 }
1229 }
1230 return 0;
1231}
1232
1234
1244void GblTrajectory::getResAndErr(unsigned int aData, bool used,
1245 double &aResidual, double &aMeasError, double &aResError,
1246 double &aDownWeight) {
1247
1248 double aMeasVar;
1249 unsigned int numLocal;
1250 unsigned int *indLocal;
1251 double *derLocal;
1252 theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, numLocal,
1253 indLocal, derLocal);
1254 VectorXd aVec(numLocal); // compressed vector of derivatives
1255 for (unsigned int j = 0; j < numLocal; ++j) {
1256 aVec[j] = derLocal[j];
1257 }
1258 MatrixXd aMat = theMatrix.getBlockMatrix(numLocal, indLocal); // compressed (covariance) matrix
1259 double aFitVar = aVec.transpose() * aMat * aVec; // variance from track fit
1260 aFitVar *= aDownWeight; // account for down-weighting (of measurement in fit)
1261 aMeasError = sqrt(aMeasVar); // error of measurement
1262 if (used)
1263 aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of (biased) residual
1264 else
1265 aResError = sqrt(aMeasVar + aFitVar); // error of (unbiased) residual
1266}
1267
1269
1276void GblTrajectory::getResAndErr(unsigned int aData, double &aResidual,
1277 double &aMeasError) {
1278
1279 double aMeasVar;
1280 theData[aData].getResidual(aResidual, aMeasVar);
1281 aMeasError = sqrt(aMeasVar); // error of measurement
1282}
1283
1286 unsigned int nBorder = numCurvature + numLocals;
1289 theMatrix.resize(numParameters, nBorder);
1291 double aValue, aWeight;
1292 unsigned int *indLocal;
1293 double *derLocal;
1294 unsigned int numLocal;
1295
1296 std::vector<GblData>::iterator itData;
1297 for (itData = theData.begin(); itData < theData.end(); ++itData) {
1298 // skipped (internal) measurement ?
1299 if (itData->getLabel() == skippedMeasLabel
1300 && itData->getType() == InternalMeasurement)
1301 continue;
1302
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;
1306 }
1307 theMatrix.addBlockMatrix(aWeight, numLocal, indLocal, derLocal);
1308 }
1309}
1310
1312
1321 unsigned int nDim = theDimension.size();
1322 // upper limit
1323 unsigned int maxData = numMeasurements + nDim * (numOffsets - 2)
1324 + externalSeed.rows();
1325 theData.reserve(maxData);
1326 measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
1327 scatDataIndex.resize(numAllPoints + 1);
1328 unsigned int nData = 0;
1329 // composed trajectory ?
1330 if (numInnerTransformations > 0) {
1331 unsigned int nInnerTransRows = innerTransformations[0].rows();
1332 unsigned int nInnerTransCols = innerTransformations[0].cols();
1333 // std::cout << "composed trajectory, inner transformation (" << nInnerTransRows << "," << nInnerTransCols << ")" << std::endl;
1334 // check size
1335 if (nInnerTransRows != 5 and nInnerTransRows != 2) {
1336 std::cout
1337 << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1338 << nInnerTransRows << "," << nInnerTransCols
1339 << "): invalid number of rows" << std::endl;
1340 throw 10;
1341 }
1342 if (nInnerTransRows > nInnerTransCols) {
1343 std::cout
1344 << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1345 << nInnerTransRows << "," << nInnerTransCols
1346 << "): too few columns" << std::endl;
1347 throw 11;
1348 }
1349 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1350 // check size
1351 if (nInnerTransRows != innerTransformations[iTraj].rows()
1352 or nInnerTransCols != innerTransformations[iTraj].cols()) {
1353 std::cout
1354 << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix["
1355 << iTraj << "]: different size as [0]" << std::endl;
1356 throw 12;
1357 }
1358 // innermost point
1359 GblPoint *innerPoint = &thePoints[iTraj].front();
1360 // transformation fit to local track parameters
1361 std::array<unsigned int, 5> firstLabels;
1362 Matrix5d matFitToLocal;
1363 getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
1364 if (nInnerTransRows == 5) {
1365 // (full) kinematic constraint
1366 // transformation local track to fit parameters
1367 Matrix5d matLocalToFit = matFitToLocal.inverse();
1368 // transformation external to fit parameters at inner (first) point
1369 innerTransDer.emplace_back(
1370 matLocalToFit * innerTransformations[iTraj]);
1371 } else {
1372 // geometric constraint (including individual curvature correction per trajectory)
1373 MatrixXd matInnerToFit(5, numCurvature);
1374 matInnerToFit.setZero();
1375 matInnerToFit(0, nInnerTransCols + iTraj) = 1.; // curvature correction for 'iTraj'
1376 matInnerToFit.block(1, 0, 2, nInnerTransCols) =
1377 innerTransformations[iTraj]; // geometric derivatives
1378 innerTransDer.emplace_back(matInnerToFit);
1379 }
1380 innerTransLab.push_back(firstLabels);
1381 }
1382 }
1383
1384 Matrix5d matP; // measurements
1385 std::vector<GblPoint>::iterator itPoint;
1386 std::vector<GblMeasurement>::const_iterator itMeas;
1387 // limit the scope of proDer:
1388 {
1389 // transform for external parameters
1390 Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor /* default */,
1391 5, 5> proDer;
1392 // loop over trajectories
1393 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1394 for (itPoint = thePoints[iTraj].begin();
1395 itPoint < thePoints[iTraj].end(); ++itPoint) {
1396 Vector5d aMeas, aPrec;
1397 unsigned int nLabel = itPoint->getLabel();
1398 if (itPoint->numMeasurements()) {
1399 for (itMeas = itPoint->getMeasBegin();
1400 itMeas < itPoint->getMeasEnd(); ++itMeas) {
1401 // skip disabled measurements
1402 if (!itMeas->isEnabled())
1403 continue;
1404 unsigned int measDim = itMeas->getMeasDim();
1405 const MatrixXd localDer = itMeas->getLocalDerivatives();
1406 maxNumGlobals = std::max(maxNumGlobals,
1407 itMeas->getNumGlobals());
1408 MatrixXd transDer;
1409 itMeas->getMeasurement(matP, aMeas, aPrec);
1410 double minPrecision = itMeas->getMeasPrecMin();
1411 unsigned int iOff = 5 - measDim; // first active component
1412 std::array<unsigned int, 5> labDer;
1413 Matrix5d matDer, matPDer;
1414 matPDer.topRows(iOff).setZero(); // clear unused part
1415 if (measDim > 2) {
1416 unsigned int nJacobian =
1417 (itPoint->isLast()) ? 0 : 1; // last point needs backward propagation (for slopes)
1418 getFitToLocalJacobian(labDer, matDer, *itPoint,
1419 measDim, nJacobian);
1420 matPDer = matP * matDer;
1421 matPDer.bottomRows(measDim) =
1422 matP.bottomRightCorner(measDim, measDim)
1423 * matDer.bottomRows(measDim);
1424 } else { // 'shortcut' for position measurements
1425 getFitToLocalJacobian(labDer, matDer, *itPoint,
1426 measDim, 1); // forward propagation (-> after THICK scatterer)
1427 matPDer.bottomRows<2>() = matP.bottomRightCorner<2,
1428 2>() * matDer.bottomRows<2>();
1429 }
1430
1431 if (numInnerTransformations > 0) {
1432 // transform for external parameters
1433 proDer.resize(measDim, Eigen::NoChange);
1434 proDer.setZero();
1435 // match parameters
1436 unsigned int ifirst = 0;
1437 unsigned int ilast = 2 * numInnerTransOffsets;
1438 unsigned int ilabel = 0;
1439 unsigned int numRelated = 0;
1440 while (ilabel < 5) {
1441 if (labDer[ilabel] > 0) {
1442 while (ifirst <= ilast
1443 and innerTransLab[iTraj][ifirst]
1444 != labDer[ilabel]) {
1445 ++ifirst;
1446 }
1447 if (ifirst > ilast) {
1448 labDer[ilabel] -= numInnerTransOffsets
1449 * nDim * (iTraj + 1); // adjust label
1450 } else {
1451 // match
1452 labDer[ilabel] = 0; // mark as related to external parameters
1453 numRelated++;
1454 for (unsigned int k = iOff; k < 5;
1455 ++k) {
1456 proDer(k - iOff, ifirst) = matPDer(
1457 k, ilabel);
1458 }
1459 }
1460 }
1461 ++ilabel;
1462 }
1463 if (numRelated > 0) {
1464 transDer.resize(measDim, numCurvature);
1465 transDer = proDer * innerTransDer[iTraj];
1466 }
1467 }
1468 for (unsigned int i = iOff; i < 5; ++i) {
1469 if (aPrec(i) > minPrecision) {
1470 GblData aData(nLabel, InternalMeasurement,
1471 aMeas(i), aPrec(i), iTraj,
1472 itPoint - thePoints[iTraj].begin(),
1473 itMeas - itPoint->getMeasBegin());
1474 aData.addDerivatives(i, labDer, matPDer, iOff,
1475 localDer, numLocals, transDer);
1476 theData.emplace_back(std::move(aData));
1477 nData++;
1478 }
1479 }
1480
1481 }
1482 }
1483 measDataIndex[nLabel] = nData;
1484 }
1485 }
1486 } // end of scope for proDer
1487
1488 Matrix4d matT; // kinks
1489 // limit the scope of proDer:
1490 {
1491 // transform for external parameters
1492 Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor /* default */,
1493 5, 5> proDer;
1494 scatDataIndex[0] = nData;
1495 scatDataIndex[1] = nData;
1496 // loop over trajectories
1497 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
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();
1503 if (scatDim) {
1504 MatrixXd transDer;
1505 std::array<unsigned int, 9> labDer;
1506 Matrix49d matDer, matTDer;
1507 unsigned int numDer;
1508 if (scatDim == 4) {
1509 // thick scatterer, last point?
1510 if (itPoint->isLast()) {
1511 // steps (only)
1512 itPoint->getReducedScatterer(matT, aMeas, aPrec);
1513 numDer = getFitToStepJacobian(labDer, matDer,
1514 *itPoint);
1515 matTDer.leftCols<4>() = matT * matDer.leftCols<4>(); // numDer == 4 !
1516 } else {
1517 // kinks+steps
1518 itPoint->getScatterer(matT, aMeas, aPrec);
1519 numDer = getFitToKinkAndStepJacobian(labDer, matDer,
1520 *itPoint);
1521 matTDer = matT * matDer; // numDer == 9 !
1522 }
1523 } else {
1524 // thin scatterer, last point?
1525 if (itPoint->isLast())
1526 break;
1527 // kinks
1528 itPoint->getScatterer(matT, aMeas, aPrec);
1529 numDer = getFitToKinkJacobian(labDer, matDer, *itPoint);
1530 matTDer.topLeftCorner<2, 7>() =
1531 matT.topLeftCorner<2, 2>()
1532 * matDer.topLeftCorner<2, 7>(); // numDer == 7 !
1533 }
1534 if (numInnerTransformations > 0) {
1535 // transform for external parameters
1536 proDer.resize(scatDim, Eigen::NoChange);
1537 proDer.setZero();
1538 // match parameters
1539 unsigned int ifirst = 0;
1540 unsigned int ilast = 2 * numInnerTransOffsets;
1541 unsigned int ilabel = 0;
1542 unsigned int numRelated = 0;
1543 while (ilabel < numDer) {
1544 if (labDer[ilabel] > 0) {
1545 while (innerTransLab[iTraj][ifirst]
1546 != labDer[ilabel] and ifirst <= ilast) {
1547 ++ifirst;
1548 }
1549 if (ifirst > ilast) {
1550 labDer[ilabel] -= numInnerTransOffsets
1551 * nDim * (iTraj + 1); // adjust label
1552 } else {
1553 // match
1554 labDer[ilabel] = 0; // mark as related to external parameters
1555 numRelated++;
1556 for (unsigned int k = 0; k < scatDim; ++k) {
1557 proDer(k, ifirst) = matTDer(k, ilabel);
1558 }
1559 }
1560 }
1561 ++ilabel;
1562 }
1563 if (numRelated > 0) {
1564 transDer.resize(scatDim, numCurvature);
1565 transDer = proDer * innerTransDer[iTraj];
1566 }
1567 }
1568 // loop over kinks and steps (if any)
1569 for (unsigned int i = 0; i < scatDim; ++i) {
1570 if (aPrec(i) > 0.) {
1571 GblData aData(nLabel, InternalKink, aMeas(i),
1572 aPrec(i), iTraj,
1573 itPoint - thePoints[iTraj].begin());
1574 aData.addDerivatives(i, numDer, labDer, matTDer,
1575 numLocals, transDer);
1576 theData.emplace_back(std::move(aData));
1577 nData++;
1578 }
1579 }
1580 }
1581 scatDataIndex[nLabel] = nData;
1582 }
1583 scatDataIndex[thePoints[iTraj].back().getLabel()] = nData;
1584 }
1585 }
1586
1587 // external seed
1588 if (externalPoint) {
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;
1597 for (int i = 0; i < externalSeed.rows(); ++i) {
1598 if (valEigen(i) > 0.) {
1599 for (int j = 0; j < externalSeed.cols(); ++j) {
1600 externalSeedDerivatives[j] = vecEigen(i, j);
1601 }
1602 GblData aData(externalPoint, ExternalSeed, 0., valEigen(i));
1603 aData.addDerivatives(externalSeedIndex,
1604 externalSeedDerivatives);
1605 theData.emplace_back(std::move(aData));
1606 nData++;
1607 }
1608 }
1609 }
1610 measDataIndex[numAllPoints + 1] = nData;
1611 // external measurements
1612 unsigned int nExt = externalMeasurements.rows();
1613 if (nExt > 0) {
1614 unsigned int nInnerTransCols = innerTransformations[0].cols();
1615 unsigned int nExtDer = externalDerivatives.cols();
1616 if (nExtDer > nInnerTransCols) {
1617 std::cout
1618 << " GblTrajectory::prepare external measurement with too many derivatives: "
1619 << nExtDer << ", defined: " << nInnerTransCols << std::endl;
1620 throw 13;
1621 }
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) {
1626 index[iCol] = numLocals + iCol + 1;
1627 derivatives[iCol] = externalDerivatives(iExt, iCol);
1628 }
1630 externalPrecisions(iExt));
1631 aData.addDerivatives(index, derivatives);
1632 theData.emplace_back(std::move(aData));
1633 nData++;
1634 }
1635 }
1636 measDataIndex[numAllPoints + 2] = nData;
1637}
1638
1641 std::vector<GblData>::iterator itData;
1642 for (itData = theData.begin(); itData < theData.end(); ++itData) {
1643 itData->setPrediction(theVector);
1644 }
1645}
1646
1648
1651double GblTrajectory::downWeight(unsigned int aMethod) {
1652 double aLoss = 0.;
1653 std::vector<GblData>::iterator itData;
1654 for (itData = theData.begin(); itData < theData.end(); ++itData) {
1655 aLoss += (1. - itData->setDownWeighting(aMethod));
1656 }
1657 return aLoss;
1658}
1659
1661
1673unsigned int GblTrajectory::fit(double &Chi2, int &Ndf, double &lostWeight,
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";
1677
1678 Chi2 = 0.;
1679 Ndf = -1;
1680 lostWeight = 0.;
1681 if (not constructOK)
1682 return 10;
1683
1684 unsigned int aMethod = 0;
1685 skippedMeasLabel = aLabel;
1686
1688 lostWeight = 0.;
1689 unsigned int ierr = 0;
1690 try {
1691
1693 predict();
1694
1695 for (unsigned int i = 0; i < optionList.size(); ++i) // down weighting iterations
1696 {
1697 size_t aPosition = methodList.find(optionList[i]);
1698 if (aPosition != std::string::npos) {
1699 aMethod = aPosition / 2 + 1;
1700 lostWeight = downWeight(aMethod);
1703 predict();
1704 }
1705 }
1706 Ndf = -numParameters;
1707 Chi2 = 0.;
1708 for (unsigned int i = 0; i < theData.size(); ++i) {
1709 // skipped (internal) measurement ?
1710 if (theData[i].getLabel() == skippedMeasLabel
1711 && theData[i].getType() == InternalMeasurement)
1712 continue;
1713 Chi2 += theData[i].getChi2();
1714 Ndf++;
1715 }
1716 Chi2 /= normChi2[aMethod];
1717 fitOK = true;
1718
1719 } catch (int e) {
1720 std::cout << " GblTrajectory::fit exception " << e << std::endl;
1721 ierr = e;
1722 }
1723 return ierr;
1724}
1725
1727
1731 double aValue;
1732 double aErr;
1733 unsigned int aTraj;
1734 unsigned int aPoint;
1735 unsigned int aMeas;
1736 unsigned int aRow;
1737 unsigned int numLocal;
1738 unsigned int *labLocal;
1739 double *derLocal;
1740 std::vector<int> labGlobal;
1741 std::vector<double> derGlobal;
1742
1743 if (not constructOK)
1744 return;
1745
1746 // data: measurements, kinks and external seed
1747 labGlobal.reserve(maxNumGlobals);
1748 derGlobal.reserve(maxNumGlobals);
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);
1753 if (itData->getType() == InternalMeasurement)
1754 thePoints[aTraj][aPoint].getGlobalLabelsAndDerivatives(aMeas, aRow,
1755 labGlobal, derGlobal);
1756 else
1757 labGlobal.resize(0);
1758 aMille.addData(aValue, aErr, numLocal, labLocal, derLocal, labGlobal,
1759 derGlobal);
1760 }
1761 aMille.writeRecord();
1762}
1763
1765
1768void GblTrajectory::printTrajectory(unsigned int level) const {
1770 std::cout << "Composed GblTrajectory, " << numInnerTransformations
1771 << " subtrajectories, type " << numInnerTransOffsets
1772 << std::endl;
1773 } else {
1774 std::cout << "Simple GblTrajectory" << std::endl;
1775 }
1776 if (theDimension.size() < 2) {
1777 std::cout << " 2D-trajectory" << std::endl;
1778 }
1779 std::cout << " Number of GblPoints : " << numAllPoints
1780 << std::endl;
1781 std::cout << " Number of points with offsets: " << numOffsetPoints
1782 << std::endl;
1783 std::cout << " Number of (1D or 2D) offsets : " << numOffsets << std::endl;
1784 std::cout << " Number of fit parameters : " << numParameters
1785 << std::endl;
1786 std::cout << " Number of measurements : " << numMeasurements
1787 << std::endl;
1788 if (externalMeasurements.rows()) {
1789 std::cout << " Number of ext. measurements : "
1790 << externalMeasurements.rows() << std::endl;
1791 }
1792 if (externalPoint) {
1793 std::cout << " Label of point with ext. seed: " << externalPoint
1794 << std::endl;
1795 }
1796 if (constructOK) {
1797 std::cout << " Constructed OK " << std::endl;
1798 }
1799 if (fitOK) {
1800 std::cout << " Fitted OK " << std::endl;
1801 }
1802 if (level > 0) {
1803 IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
1805 std::cout << " Inner transformations" << std::endl;
1806 for (unsigned int i = 0; i < numInnerTransformations; ++i) {
1807 std::cout << innerTransformations[i].format(CleanFmt)
1808 << std::endl;
1809 }
1810 }
1811 if (externalMeasurements.rows()) {
1812 std::cout << " External measurements" << std::endl;
1813 std::cout << " Measurements:" << std::endl;
1814 std::cout << externalMeasurements.format(CleanFmt) << std::endl;
1815 std::cout << " Precisions:" << std::endl;
1816 std::cout << externalPrecisions.format(CleanFmt) << std::endl;
1817 std::cout << " Derivatives:" << std::endl;
1818 std::cout << externalDerivatives.format(CleanFmt) << std::endl;
1819 }
1820 if (externalPoint) {
1821 std::cout << " External seed:" << std::endl;
1822 std::cout << externalSeed.format(CleanFmt) << std::endl;
1823 }
1824 if (fitOK) {
1825 std::cout << " Fit results" << std::endl;
1826 std::cout << " Parameters:" << std::endl;
1827 theVector.print();
1828 std::cout << " Covariance matrix (bordered band part):"
1829 << std::endl;
1831 }
1832 }
1833}
1834
1836
1839void GblTrajectory::printPoints(unsigned int level) const {
1840 std::cout << "GblPoints " << std::endl;
1841 for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1842 std::vector<GblPoint>::const_iterator itPoint;
1843 for (itPoint = thePoints[iTraj].begin();
1844 itPoint < thePoints[iTraj].end(); ++itPoint) {
1845 itPoint->printPoint(level);
1846 }
1847 }
1848}
1849
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();
1856 }
1857}
1858
1860
1865 return (fitOK) ? theMatrix.getBandCondition() : -1.;
1866}
1867
1868}
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.
Definition: GblData.h:58
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.
Definition: GblData.h:147
Point on trajectory.
Definition: GblPoint.h:62
void getDerivatives(int aDirection, Eigen::Matrix2d &matW, Eigen::Matrix2d &matWJ, Eigen::Vector2d &vecWd) const
Retrieve derivatives of local track model.
Definition: GblPoint.cpp:522
void addNextJacobian(const Matrix5d &aJac)
Define jacobian to next scatterer (by GBLTrajectory constructor)
Definition: GblPoint.cpp:508
unsigned int getScatDim() const
Get scatterer dimension.
Definition: GblPoint.cpp:365
const Matrix5d & getP2pJacobian() const
Retrieve point-to-(previous)point jacobian.
Definition: GblPoint.cpp:485
int getOffset() const
Retrieve offset for point.
Definition: GblPoint.cpp:472
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.
Definition: MilleBinary.h:81
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.
Definition: MilleBinary.cpp:73
void setZero()
Set content to 0.
Definition: VMatrix.cpp:278
void resize(const unsigned int nRows)
Resize vector.
Definition: VMatrix.cpp:272
void print() const
Print vector.
Definition: VMatrix.cpp:313
Namespace for the general broken lines package.
@ InternalMeasurement
Definition: GblData.h:50
@ ExternalMeasurement
Definition: GblData.h:50
@ ExternalSeed
Definition: GblData.h:50
@ InternalKink
Definition: GblData.h:50
Eigen::Matrix< double, 5, 1 > Vector5d
Eigen::Matrix< double, 4, 9 > Matrix49d
Definition: GblData.h:47
Eigen::Matrix< double, 5, 5 > Matrix5d
Definition: GblData.h:46