GeneralBrokenLines  Rev: 2.2.0
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 
148 #include "GblTrajectory.h"
149 using namespace Eigen;
150 
152 namespace gbl {
153 
155 
163 GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
164  bool flagCurv, bool flagU1dir, bool flagU2dir) :
165  numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTransformations(
166  0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(
167  0), numLocals(0), numMeasurements(0), externalPoint(0), skippedMeasLabel(
168  0), maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
169 
170  if (flagU1dir)
171  theDimension.push_back(0);
172  if (flagU2dir)
173  theDimension.push_back(1);
174  // simple (single) trajectory
175  thePoints.emplace_back(std::move(aPointList));
176  numPoints.push_back(numAllPoints);
177  construct(); // construct trajectory
178 }
179 
181 
185 GblTrajectory::GblTrajectory(
186  const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList) :
187  numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
188  aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
189  0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
190  0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
191 
192  for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
193  thePoints.push_back(aPointsAndTransList[iTraj].first);
194  numPoints.push_back(thePoints.back().size());
195  numAllPoints += numPoints.back();
196  innerTransformations.push_back(aPointsAndTransList[iTraj].second);
197  }
198  theDimension.push_back(0);
199  theDimension.push_back(1);
200  // kinematic (2) or geometric (1) constraint
201  numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
202  numCurvature =
203  innerTransformations[0].rows() == 5 ?
204  innerTransformations[0].cols() :
205  innerTransformations[0].cols() + numInnerTransformations;
206  construct(); // construct (composed) trajectory
207 }
208 
209 #ifdef GBL_EIGEN_SUPPORT_ROOT
211 
222 GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
223  unsigned int aLabel, const TMatrixDSym &aSeed, bool flagCurv,
224  bool flagU1dir, bool flagU2dir) :
225 numAllPoints(aPointList.size()), numPoints(), numOffsets(0),
226 numInnerTransformations(0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
227 numLocals(0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(0),
228 maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(),
229 scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(),
230 externalMeasurements(), externalPrecisions() {
231 
232  if (flagU1dir)
233  theDimension.push_back(0);
234  if (flagU2dir)
235  theDimension.push_back(1);
236  // convert from ROOT
237  unsigned int nParSeed = aSeed.GetNrows();
238  externalSeed.resize(nParSeed, nParSeed);
239  for (unsigned int i = 0; i < nParSeed; ++i) {
240  for (unsigned int j = 0; j < nParSeed; ++j) {
241  externalSeed(i, j) = aSeed(i, j);
242  }
243  }
244  // simple (single) trajectory
245  thePoints.emplace_back(std::move(aPointList));
246  numPoints.push_back(numAllPoints);
247  construct();// construct trajectory
248 }
249 
251 
255 GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList) :
256 numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
257 numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
258 skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
259 measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
260 externalDerivatives(), externalMeasurements(), externalPrecisions() {
261 
262  for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
263  thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
264  numPoints.push_back(thePoints.back().size());
265  numAllPoints += numPoints.back();
266  // convert from ROOT
267  unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
268  unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
269  MatrixXd aTrans(nRowTrans, nColTrans);
270  for (unsigned int i = 0; i < nRowTrans; ++i) {
271  for (unsigned int j = 0; j < nColTrans; ++j) {
272  aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
273  }
274  }
275  innerTransformations.emplace_back(std::move(aTrans));
276  }
277  theDimension.push_back(0);
278  theDimension.push_back(1);
279  // kinematic (2) or geometric (1) constraint
280  numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
281  numCurvature =
282  innerTransformations[0].rows() == 5 ?
283  innerTransformations[0].cols() :
284  innerTransformations[0].cols() + numInnerTransformations;
285  construct();// construct (composed) trajectory
286 }
287 
289 
296 GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
297  const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
298  const TVectorD &extPrecisions) :
299 numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
300 numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
301 skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
302 measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
303 externalDerivatives(), externalMeasurements(), externalPrecisions() {
304 
305  // convert from ROOT
306  unsigned int nExtMeas = extDerivatives.GetNrows();
307  unsigned int nExtPar = extDerivatives.GetNcols();
308  externalDerivatives.resize(nExtMeas, nExtPar);
309  externalMeasurements.resize(nExtMeas);
310  externalPrecisions.resize(nExtMeas);
311  for (unsigned int i = 0; i < nExtMeas; ++i) {
312  externalMeasurements(i) = extMeasurements[i];
313  externalPrecisions(i) = extPrecisions[i];
314  for (unsigned int j = 0; j < nExtPar; ++j) {
315  externalDerivatives(i, j) = extDerivatives(i, j);
316  }
317  }
318  for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
319  thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
320  numPoints.push_back(thePoints.back().size());
321  numAllPoints += numPoints.back();
322  // convert from ROOT
323  unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
324  unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
325  MatrixXd aTrans(nRowTrans, nColTrans);
326  for (unsigned int i = 0; i < nRowTrans; ++i) {
327  for (unsigned int j = 0; j < nColTrans; ++j) {
328  aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
329  }
330  }
331  innerTransformations.emplace_back(std::move(aTrans));
332  }
333  theDimension.push_back(0);
334  theDimension.push_back(1);
335  // kinematic (2) or geometric (1) constraint
336  numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
337  numCurvature =
338  innerTransformations[0].rows() == 5 ?
339  innerTransformations[0].cols() :
340  innerTransformations[0].cols() + numInnerTransformations;
341  construct();// construct (composed) trajectory
342 }
343 
345 
352 GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
353  const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
354  const TMatrixDSym &extPrecisions) :
355 numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
356 numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
357 skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
358 measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
359 
360  // diagonalize external measurement
361  TMatrixDSymEigen extEigen(extPrecisions);
362  TMatrixD extTransformation = extEigen.GetEigenVectors();
363  extTransformation.T();
364  TMatrixD aDerivatives = extTransformation * extDerivatives;
365  TVectorD aMeasurements = extTransformation * extMeasurements;
366  TVectorD aPrecisions = extEigen.GetEigenValues();
367  // convert from ROOT
368  unsigned int nExtMeas = aDerivatives.GetNrows();
369  unsigned int nExtPar = aDerivatives.GetNcols();
370  externalDerivatives.resize(nExtMeas, nExtPar);
371  externalMeasurements.resize(nExtMeas);
372  externalPrecisions.resize(nExtMeas);
373  for (unsigned int i = 0; i < nExtMeas; ++i) {
374  externalMeasurements(i) = aMeasurements[i];
375  externalPrecisions(i) = aPrecisions[i];
376  for (unsigned int j = 0; j < nExtPar; ++j) {
377  externalDerivatives(i, j) = aDerivatives(i, j);
378  }
379  }
380  for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
381  thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
382  numPoints.push_back(thePoints.back().size());
383  numAllPoints += numPoints.back();
384  // convert from ROOT
385  unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
386  unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
387  MatrixXd aTrans(nRowTrans, nColTrans);
388  for (unsigned int i = 0; i < nRowTrans; ++i) {
389  for (unsigned int j = 0; j < nColTrans; ++j) {
390  aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
391  }
392  }
393  innerTransformations.emplace_back(std::move(aTrans));
394  }
395  theDimension.push_back(0);
396  theDimension.push_back(1);
397  // kinematic (2) or geometric (1) constraint
398  numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
399  numCurvature =
400  innerTransformations[0].rows() == 5 ?
401  innerTransformations[0].cols() :
402  innerTransformations[0].cols() + numInnerTransformations;
403  construct();// construct (composed) trajectory
404 }
405 #endif
406 
407 GblTrajectory::~GblTrajectory() {
408 }
409 
411 bool GblTrajectory::isValid() const {
412  return constructOK;
413 }
414 
416 unsigned int GblTrajectory::getNumPoints() const {
417  return numAllPoints;
418 }
419 
421 
424 void GblTrajectory::construct() {
425 
426  constructOK = false;
427  fitOK = false;
428  unsigned int aLabel = 0;
429  if (numAllPoints < 2) {
430  std::cout << " GblTrajectory construction failed: too few GblPoints "
431  << std::endl;
432  return;
433  }
434  // loop over trajectories
435  numTrajectories = thePoints.size();
436  //std::cout << " numTrajectories: " << numTrajectories << ", " << innerTransformations.size() << std::endl;
437  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
438  std::vector<GblPoint>::iterator itPoint;
439  for (itPoint = thePoints[iTraj].begin();
440  itPoint < thePoints[iTraj].end(); ++itPoint) {
441  numLocals = std::max(numLocals, itPoint->getNumLocals());
442  numMeasurements += itPoint->hasMeasurement();
443  itPoint->setLabel(++aLabel);
444  }
445  }
446  defineOffsets();
447  calcJacobians();
448  try {
449  prepare();
450  } catch (std::overflow_error &e) {
451  std::cout << " GblTrajectory construction failed: " << e.what()
452  << std::endl;
453  return;
454  } catch (int i) {
455  std::cout << " GblTrajectory construction failed with exception " << i
456  << std::endl;
457  return;
458  }
459 
460  constructOK = true;
461  // number of fit parameters
462  numParameters =
463  (numOffsets - numInnerTransOffsets * numInnerTransformations)
464  * theDimension.size() + numCurvature + numLocals;
465 }
466 
468 
472 void GblTrajectory::defineOffsets() {
473 
474  // loop over trajectories
475  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
476  // first point is offset
477  thePoints[iTraj].front().setOffset(numOffsets++);
478  // intermediate scatterers are offsets
479  std::vector<GblPoint>::iterator itPoint;
480  for (itPoint = thePoints[iTraj].begin() + 1;
481  itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
482  if (itPoint->hasScatterer()) {
483  itPoint->setOffset(numOffsets++);
484  } else {
485  itPoint->setOffset(-numOffsets);
486  }
487  }
488  // last point is offset
489  thePoints[iTraj].back().setOffset(numOffsets++);
490  }
491 }
492 
494 void GblTrajectory::calcJacobians() {
495 
496  Matrix5d scatJacobian;
497  // loop over trajectories
498  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
499  // forward propagation (all)
500  GblPoint* previousPoint = &thePoints[iTraj].front();
501  unsigned int numStep = 0;
502  std::vector<GblPoint>::iterator itPoint;
503  for (itPoint = thePoints[iTraj].begin() + 1;
504  itPoint < thePoints[iTraj].end(); ++itPoint) {
505  if (numStep == 0) {
506  scatJacobian = itPoint->getP2pJacobian();
507  } else {
508  scatJacobian = itPoint->getP2pJacobian() * scatJacobian;
509  }
510  numStep++;
511  itPoint->addPrevJacobian(scatJacobian); // iPoint -> previous scatterer
512  if (itPoint->getOffset() >= 0) {
513  previousPoint->addNextJacobian(scatJacobian); // lastPoint -> next scatterer
514  numStep = 0;
515  previousPoint = &(*itPoint);
516  }
517  }
518  // backward propagation (without scatterers)
519  for (itPoint = thePoints[iTraj].end() - 1;
520  itPoint > thePoints[iTraj].begin(); --itPoint) {
521  if (itPoint->getOffset() >= 0) {
522  scatJacobian = itPoint->getP2pJacobian();
523  continue; // skip offsets
524  }
525  itPoint->addNextJacobian(scatJacobian); // iPoint -> next scatterer
526  scatJacobian = scatJacobian * itPoint->getP2pJacobian();
527  }
528  }
529 }
530 
532 
540 std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
541  int aSignedLabel) const {
542 
543  unsigned int nDim = theDimension.size();
544  unsigned int nCurv = numCurvature;
545  unsigned int nLocals = numLocals;
546  // find trajectory, position in trajectory
547  unsigned int aLabel = abs(aSignedLabel);
548  unsigned int firstLabel = 1;
549  unsigned int lastLabel = 0;
550  unsigned int aTrajectory = 0;
551  // loop over trajectories
552  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
553  aTrajectory = iTraj;
554  lastLabel += numPoints[iTraj];
555  if (aLabel <= lastLabel)
556  break;
557  if (iTraj < numTrajectories - 1)
558  firstLabel += numPoints[iTraj];
559  }
560  int nJacobian; // 0: prev, 1: next
561  // check consistency of (index, direction)
562  if (aSignedLabel > 0) {
563  nJacobian = 1;
564  if (aLabel >= lastLabel) {
565  aLabel = lastLabel;
566  nJacobian = 0;
567  }
568  } else {
569  nJacobian = 0;
570  if (aLabel <= firstLabel) {
571  aLabel = firstLabel;
572  nJacobian = 1;
573  }
574  }
575 
576  const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
577  std::array<unsigned int, 5> labDer;
578  Matrix5d matDer;
579  getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);
580 
581  unsigned int nBand = 0; // number of parameters from band part
582  if (numInnerTransformations > 0) {
583  unsigned int lastExt = innerTransLab[aTrajectory][2
584  * numInnerTransOffsets]; // last label for external parameters
585  for (unsigned int i = 0; i < 5; ++i)
586  if (labDer[i] > lastExt)
587  nBand++;
588  } else {
589  nBand = 2 * nDim;
590  }
591 
592  unsigned int nBorder = nCurv + nLocals;
593  unsigned int nParBRL = nBorder + nBand;
594  unsigned int nParLoc = nLocals + 5;
595  std::vector<unsigned int> anIndex;
596  anIndex.reserve(nParBRL);
597  MatrixXd aJacobian(nParLoc, nParBRL);
598  aJacobian.setZero();
599 
600  // from local parameters
601  for (unsigned int i = 0; i < nLocals; ++i) {
602  aJacobian(i + 5, i) = 1.0;
603  anIndex.push_back(i + 1);
604  }
605  // from trajectory parameters
606  unsigned int iCol = nLocals;
607  // composed trajectory ?
608  if (numInnerTransformations > 0) {
609  // transformation external to (simple) broken line (fit) parameters
610  unsigned int nSimple = nCurv + nBand;
611  MatrixXd aTrans(5, nSimple);
612  aTrans.setZero();
613  // external part, curvature
614  aTrans.block(0, 0, 1, nCurv) = innerTransDer[aTrajectory].block(0, 0, 1,
615  nCurv);
616  for (unsigned int i = 0; i < nCurv; ++i)
617  anIndex.push_back(++iCol);
618  if (nBand < 4) {
619  // external part, offsets (nBand=0: 2, nBand=2: 1)
620  unsigned int iRow = 1 + 2 * numInnerTransOffsets + nBand - 4; // start row (1: 1st, 3: 2nd offset)
621  aTrans.block(1, 0, 4 - nBand, nCurv) =
622  innerTransDer[aTrajectory].block(iRow, 0, 4 - nBand, nCurv);
623  }
624  // (remaining) band part
625  for (unsigned int i = 5 - nBand; i < 5; ++i) {
626  aTrans(i, iCol++) = 1.;
627  anIndex.push_back(
628  labDer[i]
629  - numInnerTransOffsets * nDim * (aTrajectory + 1)); // adjust label
630  }
631  aJacobian.block(0, nLocals, 5, nSimple) = matDer * aTrans;
632  } else {
633  // simple trajectory
634  for (unsigned int i = 0; i < 5; ++i) {
635  if (labDer[i] > 0) {
636  anIndex.push_back(labDer[i]);
637  for (unsigned int j = 0; j < 5; ++j) {
638  aJacobian(j, iCol) = matDer(j, i);
639  }
640  ++iCol;
641  }
642  }
643  }
644  return std::make_pair(anIndex, aJacobian);
645 }
646 
648 
657 void GblTrajectory::getFitToLocalJacobian(std::array<unsigned int, 5>& anIndex,
658  Matrix5d &aJacobian, const GblPoint &aPoint, unsigned int measDim,
659  unsigned int nJacobian) const {
660 
661  unsigned int nDim = theDimension.size();
662  unsigned int nCurv = numCurvature;
663  unsigned int nLocals = numLocals;
664 
665  int nOffset = aPoint.getOffset();
666 
667  anIndex = {}; // reset to 0
668  aJacobian.setZero();
669  if (nOffset < 0) // need interpolation
670  {
671  Matrix2d prevW, prevWJ, nextW, nextWJ, matN;
672  Vector2d prevWd, nextWd;
673  aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
674  aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W+, W+ * J+, W+ * d+
675  const Matrix2d sumWJ(prevWJ + nextWJ);
676  matN = sumWJ.inverse(); // N = (W- * J- + W+ * J+)^-1
677  // derivatives for u_int
678  const Matrix2d prevNW(matN * prevW); // N * W-
679  const Matrix2d nextNW(matN * nextW); // N * W+
680  const Vector2d prevNd(matN * prevWd); // N * W- * d-
681  const Vector2d nextNd(matN * nextWd); // N * W+ * d+
682 
683  unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i)
684 
685  // local offset
686  if (nCurv > 0) {
687  aJacobian.block<2, 1>(3, 0) = -prevNd - nextNd; // from curvature
688  anIndex[0] = nLocals + 1;
689  }
690  aJacobian.block<2, 2>(3, 1) = prevNW; // from 1st Offset
691  aJacobian.block<2, 2>(3, 3) = nextNW; // from 2nd Offset
692  for (unsigned int i = 0; i < nDim; ++i) {
693  anIndex[1 + theDimension[i]] = iOff + i;
694  anIndex[3 + theDimension[i]] = iOff + nDim + i;
695  }
696 
697  // local slope and curvature
698  if (measDim > 2) {
699  // derivatives for u'_int
700  const Matrix2d prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
701  const Matrix2d nextWPN(prevWJ * nextNW); // W- * J- * N * W+
702  const Vector2d prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
703  const Vector2d nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
704  if (nCurv > 0) {
705  aJacobian(0, 0) = 1.0;
706  aJacobian.block<2, 1>(1, 0) = prevWNd - nextWNd; // from curvature
707  }
708  aJacobian.block<2, 2>(1, 1) = -prevWPN; // from 1st Offset
709  aJacobian.block<2, 2>(1, 3) = nextWPN; // from 2nd Offset
710  }
711  } else { // at point
712  // anIndex must be sorted
713  // forward : iOff2 = iOff1 + nDim, index1 = 1, index2 = 3
714  // backward: iOff2 = iOff1 - nDim, index1 = 3, index2 = 1
715  unsigned int iOff1 = nDim * nOffset + nCurv + nLocals + 1; // first offset ('i' in u_i)
716  unsigned int index1 = 3 - 2 * nJacobian; // index of first offset
717  unsigned int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1); // second offset ('i' in u_i)
718  unsigned int index2 = 1 + 2 * nJacobian; // index of second offset
719  // local offset
720  aJacobian(3, index1) = 1.0; // from 1st Offset
721  aJacobian(4, index1 + 1) = 1.0;
722  for (unsigned int i = 0; i < nDim; ++i) {
723  anIndex[index1 + theDimension[i]] = iOff1 + i;
724  }
725 
726  // local slope and curvature
727  if (measDim > 2) {
728  Matrix2d matW, matWJ;
729  Vector2d vecWd;
730  aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d
731  double sign = (nJacobian > 0) ? 1. : -1.;
732  if (nCurv > 0) {
733  aJacobian(0, 0) = 1.0;
734  aJacobian.block<2, 1>(1, 0) = -sign * vecWd; // from curvature
735  anIndex[0] = nLocals + 1;
736  }
737  aJacobian.block<2, 2>(1, index1) = -sign * matWJ; // from 1st Offset
738  aJacobian.block<2, 2>(1, index2) = sign * matW; // from 2nd Offset
739  for (unsigned int i = 0; i < nDim; ++i) {
740  anIndex[index2 + theDimension[i]] = iOff2 + i;
741  }
742  }
743  }
744 }
745 
747 
753 void GblTrajectory::getFitToKinkJacobian(std::array<unsigned int, 7>& anIndex,
754  Matrix27d &aJacobian, const GblPoint &aPoint) const {
755 
756  unsigned int nDim = theDimension.size();
757  unsigned int nCurv = numCurvature;
758  unsigned int nLocals = numLocals;
759 
760  int nOffset = aPoint.getOffset();
761 
762  anIndex = {}; // reset to 0
763  aJacobian.setZero();
764 
765  Matrix2d prevW, prevWJ, nextW, nextWJ;
766  Vector2d prevWd, nextWd;
767  aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
768  aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
769  const Matrix2d sumWJ(prevWJ + nextWJ); // W- * J- + W+ * J+
770  const Vector2d sumWd(prevWd + nextWd); // W+ * d+ + W- * d-
771 
772  unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)
773 
774  // local offset
775  if (nCurv > 0) {
776  aJacobian.block<2, 1>(0, 0) = -sumWd; // from curvature
777  anIndex[0] = nLocals + 1;
778  }
779  aJacobian.block<2, 2>(0, 1) = prevW; // from 1st Offset
780  aJacobian.block<2, 2>(0, 3) = -sumWJ; // from 2nd Offset
781  aJacobian.block<2, 2>(0, 5) = nextW; // from 1st Offset
782  for (unsigned int i = 0; i < nDim; ++i) {
783  anIndex[1 + theDimension[i]] = iOff + i;
784  anIndex[3 + theDimension[i]] = iOff + nDim + i;
785  anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i;
786  }
787 }
788 
790 
804 unsigned int GblTrajectory::getResults(int aSignedLabel,
805  Eigen::VectorXd &localPar, Eigen::MatrixXd &localCov) const {
806  if (not fitOK)
807  return 1;
808  std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
809  getJacobian(aSignedLabel);
810  unsigned int nParBrl = indexAndJacobian.first.size();
811  VectorXd aVec(nParBrl); // compressed vector
812  for (unsigned int i = 0; i < nParBrl; ++i) {
813  aVec[i] = theVector(indexAndJacobian.first[i] - 1);
814  }
815  MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
816  localPar = indexAndJacobian.second * aVec;
817  localCov = indexAndJacobian.second * aMat
818  * indexAndJacobian.second.adjoint();
819  return 0;
820 }
821 
823 
835 unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
836  unsigned int &numData, Eigen::VectorXd &aResiduals,
837  Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
838  Eigen::VectorXd &aDownWeights) {
839  numData = 0;
840  if (not fitOK)
841  return 1;
842 
843  unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
844  numData = measDataIndex[aLabel] - firstData; // number of data blocks
845  for (unsigned int i = 0; i < numData; ++i) {
846  getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals(i),
847  aMeasErrors(i), aResErrors(i), aDownWeights(i));
848  }
849  return 0;
850 }
851 
853 
865 unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
866  unsigned int &numData, Eigen::VectorXd &aResiduals,
867  Eigen::VectorXd &aMeasErrors, Eigen::VectorXd &aResErrors,
868  Eigen::VectorXd &aDownWeights) {
869  numData = 0;
870  if (not fitOK)
871  return 1;
872 
873  unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
874  numData = scatDataIndex[aLabel] - firstData; // number of data blocks
875  for (unsigned int i = 0; i < numData; ++i) {
876  getResAndErr(firstData + i, true, aResiduals(i), aMeasErrors(i),
877  aResErrors(i), aDownWeights(i));
878  }
879  return 0;
880 }
881 
882 #ifdef GBL_EIGEN_SUPPORT_ROOT
884 
898 unsigned int GblTrajectory::getResults(int aSignedLabel, TVectorD &localPar,
899  TMatrixDSym &localCov) const {
900  if (not fitOK)
901  return 1;
902  std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
903  getJacobian(aSignedLabel);
904  unsigned int nParBrl = indexAndJacobian.first.size();
905  VectorXd aVec(nParBrl); // compressed vector
906  for (unsigned int i = 0; i < nParBrl; ++i) {
907  aVec[i] = theVector(indexAndJacobian.first[i] - 1);
908  }
909  MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
910  VectorXd aLocalPar = indexAndJacobian.second * aVec;
911  MatrixXd aLocalCov = indexAndJacobian.second * aMat
912  * indexAndJacobian.second.adjoint();
913  // convert to ROOT
914  unsigned int nParOut = localPar.GetNrows();
915  for (unsigned int i = 0; i < nParOut; ++i) {
916  localPar[i] = aLocalPar(i);
917  for (unsigned int j = 0; j < nParOut; ++j) {
918  localCov(i, j) = aLocalCov(i, j);
919  }
920  }
921  return 0;
922 }
923 
925 
937 unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
938  unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
939  TVectorD &aResErrors, TVectorD &aDownWeights) {
940  numData = 0;
941  if (not fitOK)
942  return 1;
943 
944  unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
945  numData = measDataIndex[aLabel] - firstData;// number of data blocks
946  for (unsigned int i = 0; i < numData; ++i) {
947  getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals[i],
948  aMeasErrors[i], aResErrors[i], aDownWeights[i]);
949  }
950  return 0;
951 }
952 
954 
966 unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
967  unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
968  TVectorD &aResErrors, TVectorD &aDownWeights) {
969  numData = 0;
970  if (not fitOK)
971  return 1;
972 
973  unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
974  numData = scatDataIndex[aLabel] - firstData;// number of data blocks
975  for (unsigned int i = 0; i < numData; ++i) {
976  getResAndErr(firstData + i, true, aResiduals[i], aMeasErrors[i],
977  aResErrors[i], aDownWeights[i]);
978  }
979  return 0;
980 }
981 
982 #endif
983 
985 
989 unsigned int GblTrajectory::getLabels(
990  std::vector<unsigned int> &aLabelList) const {
991  if (not constructOK)
992  return 1;
993 
994  unsigned int aLabel = 0;
995  unsigned int nPoint = thePoints[0].size();
996  aLabelList.resize(nPoint);
997  for (unsigned i = 0; i < nPoint; ++i) {
998  aLabelList[i] = ++aLabel;
999  }
1000  return 0;
1001 }
1002 
1004 
1008 unsigned int GblTrajectory::getLabels(
1009  std::vector<std::vector<unsigned int> > &aLabelList) const {
1010  if (not constructOK)
1011  return 1;
1012 
1013  unsigned int aLabel = 0;
1014  aLabelList.resize(numTrajectories);
1015  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1016  unsigned int nPoint = thePoints[iTraj].size();
1017  aLabelList[iTraj].resize(nPoint);
1018  for (unsigned i = 0; i < nPoint; ++i) {
1019  aLabelList[iTraj][i] = ++aLabel;
1020  }
1021  }
1022  return 0;
1023 }
1024 
1026 
1036 void GblTrajectory::getResAndErr(unsigned int aData, bool used,
1037  double &aResidual, double &aMeasError, double &aResError,
1038  double &aDownWeight) {
1039 
1040  double aMeasVar;
1041  unsigned int numLocal;
1042  unsigned int* indLocal;
1043  double* derLocal;
1044  theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, numLocal,
1045  indLocal, derLocal);
1046  VectorXd aVec(numLocal); // compressed vector of derivatives
1047  for (unsigned int j = 0; j < numLocal; ++j) {
1048  aVec[j] = derLocal[j];
1049  }
1050  MatrixXd aMat = theMatrix.getBlockMatrix(numLocal, indLocal); // compressed (covariance) matrix
1051  double aFitVar = aVec.transpose() * aMat * aVec; // variance from track fit
1052  aFitVar *= aDownWeight; // account for down-weighting (of measurement in fit)
1053  aMeasError = sqrt(aMeasVar); // error of measurement
1054  if (used)
1055  aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of (biased) residual
1056  else
1057  aResError = sqrt(aMeasVar + aFitVar); // error of (unbiased) residual
1058 }
1059 
1061 void GblTrajectory::buildLinearEquationSystem() {
1062  unsigned int nBorder = numCurvature + numLocals;
1063  theVector.resize(numParameters);
1064  theMatrix.resize(numParameters, nBorder);
1065  double aValue, aWeight;
1066  unsigned int* indLocal;
1067  double* derLocal;
1068  unsigned int numLocal;
1069 
1070  std::vector<GblData>::iterator itData;
1071  for (itData = theData.begin(); itData < theData.end(); ++itData) {
1072  // skipped (internal) measurement ?
1073  if (itData->getLabel() == skippedMeasLabel
1074  && itData->getType() == InternalMeasurement)
1075  continue;
1076 
1077  itData->getLocalData(aValue, aWeight, numLocal, indLocal, derLocal);
1078  for (unsigned int j = 0; j < numLocal; ++j) {
1079  theVector(indLocal[j] - 1) += derLocal[j] * aWeight * aValue;
1080  }
1081  theMatrix.addBlockMatrix(aWeight, numLocal, indLocal, derLocal);
1082  }
1083 }
1084 
1086 
1093 void GblTrajectory::prepare() {
1094  unsigned int nDim = theDimension.size();
1095  // upper limit
1096  unsigned int maxData = numMeasurements + nDim * (numOffsets - 2)
1097  + externalSeed.rows();
1098  theData.reserve(maxData);
1099  measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
1100  scatDataIndex.resize(numAllPoints + 1);
1101  unsigned int nData = 0;
1102  // composed trajectory ?
1103  if (numInnerTransformations > 0) {
1104  unsigned int nInnerTransRows = innerTransformations[0].rows();
1105  unsigned int nInnerTransCols = innerTransformations[0].cols();
1106  // std::cout << "composed trajectory, inner transformation (" << nInnerTransRows << "," << nInnerTransCols << ")" << std::endl;
1107  // check size
1108  if (nInnerTransRows != 5 and nInnerTransRows != 2) {
1109  std::cout
1110  << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1111  << nInnerTransRows << "," << nInnerTransCols
1112  << "): invalid number of rows" << std::endl;
1113  throw 10;
1114  }
1115  if (nInnerTransRows > nInnerTransCols) {
1116  std::cout
1117  << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
1118  << nInnerTransRows << "," << nInnerTransCols
1119  << "): too few columns" << std::endl;
1120  throw 11;
1121  }
1122  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1123  // check size
1124  if (nInnerTransRows != innerTransformations[iTraj].rows()
1125  or nInnerTransCols != innerTransformations[iTraj].cols()) {
1126  std::cout
1127  << " GblTrajectory::prepare composed trajectory with bad inner transformation matrix["
1128  << iTraj << "]: different size as [0]" << std::endl;
1129  throw 12;
1130  }
1131  // innermost point
1132  GblPoint* innerPoint = &thePoints[iTraj].front();
1133  // transformation fit to local track parameters
1134  std::array<unsigned int, 5> firstLabels;
1135  Matrix5d matFitToLocal;
1136  getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
1137  if (nInnerTransRows == 5) {
1138  // (full) kinematic constraint
1139  // transformation local track to fit parameters
1140  Matrix5d matLocalToFit = matFitToLocal.inverse();
1141  // transformation external to fit parameters at inner (first) point
1142  innerTransDer.emplace_back(
1143  matLocalToFit * innerTransformations[iTraj]);
1144  } else {
1145  // geometric constraint (including individual curvature correction per trajectory)
1146  MatrixXd matInnerToFit(5, numCurvature);
1147  matInnerToFit.setZero();
1148  matInnerToFit(0, nInnerTransCols + iTraj) = 1.; // curvature correction for 'iTraj'
1149  matInnerToFit.block(1, 0, 2, nInnerTransCols) =
1150  innerTransformations[iTraj]; // geometric derivatives
1151  innerTransDer.emplace_back(matInnerToFit);
1152  }
1153  innerTransLab.push_back(firstLabels);
1154  }
1155  }
1156 
1157  Matrix5d matP; // measurements
1158  std::vector<GblPoint>::iterator itPoint;
1159  // limit the scope of proDer:
1160  {
1161  // transform for external parameters
1162  Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor /* default */,
1163  5, 5> proDer;
1164  // loop over trajectories
1165  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1166  for (itPoint = thePoints[iTraj].begin();
1167  itPoint < thePoints[iTraj].end(); ++itPoint) {
1168  Vector5d aMeas, aPrec;
1169  unsigned int nLabel = itPoint->getLabel();
1170  unsigned int measDim = itPoint->hasMeasurement();
1171  if (measDim) {
1172  const MatrixXd localDer = itPoint->getLocalDerivatives();
1173  maxNumGlobals = std::max(maxNumGlobals,
1174  itPoint->getNumGlobals());
1175  MatrixXd transDer;
1176  itPoint->getMeasurement(matP, aMeas, aPrec);
1177  double minPrecision = itPoint->getMeasPrecMin();
1178  unsigned int iOff = 5 - measDim; // first active component
1179  std::array<unsigned int, 5> labDer;
1180  Matrix5d matDer, matPDer;
1181  unsigned int nJacobian =
1182  (itPoint < thePoints[iTraj].end() - 1) ? 1 : 0; // last point needs backward propagation
1183  getFitToLocalJacobian(labDer, matDer, *itPoint, measDim,
1184  nJacobian);
1185  if (measDim > 2) {
1186  matPDer = matP * matDer;
1187  } else { // 'shortcut' for position measurements
1188  matPDer.setZero();
1189  matPDer.block<2, 5>(3, 0) = matP.block<2, 2>(3, 3)
1190  * matDer.block<2, 5>(3, 0);
1191  }
1192 
1193  if (numInnerTransformations > 0) {
1194  // transform for external parameters
1195  proDer.resize(measDim, Eigen::NoChange);
1196  proDer.setZero();
1197  // match parameters
1198  unsigned int ifirst = 0;
1199  unsigned int ilast = 2 * numInnerTransOffsets;
1200  unsigned int ilabel = 0;
1201  unsigned int numRelated = 0;
1202  while (ilabel < 5) {
1203  if (labDer[ilabel] > 0) {
1204  while (innerTransLab[iTraj][ifirst]
1205  != labDer[ilabel] and ifirst <= ilast) {
1206  ++ifirst;
1207  }
1208  if (ifirst > ilast) {
1209  labDer[ilabel] -= numInnerTransOffsets
1210  * nDim * (iTraj + 1); // adjust label
1211  } else {
1212  // match
1213  labDer[ilabel] = 0; // mark as related to external parameters
1214  numRelated++;
1215  for (unsigned int k = iOff; k < 5; ++k) {
1216  proDer(k - iOff, ifirst) = matPDer(k,
1217  ilabel);
1218  }
1219  }
1220  }
1221  ++ilabel;
1222  }
1223  if (numRelated > 0) {
1224  transDer.resize(measDim, numCurvature);
1225  transDer = proDer * innerTransDer[iTraj];
1226  }
1227  }
1228  for (unsigned int i = iOff; i < 5; ++i) {
1229  if (aPrec(i) > minPrecision) {
1230  GblData aData(nLabel, InternalMeasurement, aMeas(i),
1231  aPrec(i), iTraj,
1232  itPoint - thePoints[iTraj].begin());
1233  aData.addDerivatives(i, labDer, matPDer, iOff,
1234  localDer, numLocals, transDer);
1235  theData.emplace_back(std::move(aData));
1236  nData++;
1237  }
1238  }
1239 
1240  }
1241  measDataIndex[nLabel] = nData;
1242  }
1243  }
1244  } // end of scope for proDer
1245 
1246  Matrix2d matT; // kinks
1247  // limit the scope of proDer:
1248  {
1249  // transform for external parameters
1250  Eigen::Matrix<double, Eigen::Dynamic, 5, Eigen::ColMajor /* default */,
1251  5, 5> proDer;
1252  scatDataIndex[0] = nData;
1253  scatDataIndex[1] = nData;
1254  // loop over trajectories
1255  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1256  for (itPoint = thePoints[iTraj].begin() + 1;
1257  itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
1258  Vector2d aMeas, aPrec;
1259  unsigned int nLabel = itPoint->getLabel();
1260  if (itPoint->hasScatterer()) {
1261  itPoint->getScatterer(matT, aMeas, aPrec);
1262  MatrixXd transDer;
1263  std::array<unsigned int, 7> labDer;
1264  Matrix27d matDer, matTDer;
1265  getFitToKinkJacobian(labDer, matDer, *itPoint);
1266  matTDer = matT * matDer;
1267  if (numInnerTransformations > 0) {
1268  // transform for external parameters
1269  proDer.resize(nDim, Eigen::NoChange);
1270  proDer.setZero();
1271  // match parameters
1272  unsigned int ifirst = 0;
1273  unsigned int ilast = 2 * numInnerTransOffsets;
1274  unsigned int ilabel = 0;
1275  unsigned int numRelated = 0;
1276  while (ilabel < 7) {
1277  if (labDer[ilabel] > 0) {
1278  while (innerTransLab[iTraj][ifirst]
1279  != labDer[ilabel] and ifirst <= ilast) {
1280  ++ifirst;
1281  }
1282  if (ifirst > ilast) {
1283  labDer[ilabel] -= numInnerTransOffsets
1284  * nDim * (iTraj + 1); // adjust label
1285  } else {
1286  // match
1287  labDer[ilabel] = 0; // mark as related to external parameters
1288  numRelated++;
1289  for (unsigned int k = 0; k < nDim; ++k) {
1290  proDer(k, ifirst) = matTDer(k, ilabel);
1291  }
1292  }
1293  }
1294  ++ilabel;
1295  }
1296  if (numRelated > 0) {
1297  transDer.resize(nDim, numCurvature);
1298  transDer = proDer * innerTransDer[iTraj];
1299  }
1300  }
1301  for (unsigned int i = 0; i < nDim; ++i) {
1302  unsigned int iDim = theDimension[i];
1303  if (aPrec(iDim) > 0.) {
1304  GblData aData(nLabel, InternalKink, aMeas(iDim),
1305  aPrec(iDim), iTraj,
1306  itPoint - thePoints[iTraj].begin());
1307  aData.addDerivatives(iDim, labDer, matTDer,
1308  numLocals, transDer);
1309  theData.emplace_back(std::move(aData));
1310  nData++;
1311  }
1312  }
1313  }
1314  scatDataIndex[nLabel] = nData;
1315  }
1316  scatDataIndex[thePoints[iTraj].back().getLabel()] = nData;
1317  }
1318  }
1319 
1320  // external seed
1321  if (externalPoint > 0) {
1322  std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
1323  getJacobian(externalPoint);
1324  std::vector<unsigned int> externalSeedIndex = indexAndJacobian.first;
1325  std::vector<double> externalSeedDerivatives(externalSeedIndex.size());
1326  SelfAdjointEigenSolver<MatrixXd> externalSeedEigen(externalSeed);
1327  VectorXd valEigen = externalSeedEigen.eigenvalues();
1328  MatrixXd vecEigen = externalSeedEigen.eigenvectors();
1329  vecEigen = vecEigen.transpose() * indexAndJacobian.second;
1330  for (int i = 0; i < externalSeed.rows(); ++i) {
1331  if (valEigen(i) > 0.) {
1332  for (int j = 0; j < externalSeed.cols(); ++j) {
1333  externalSeedDerivatives[j] = vecEigen(i, j);
1334  }
1335  GblData aData(externalPoint, ExternalSeed, 0., valEigen(i));
1336  aData.addDerivatives(externalSeedIndex,
1337  externalSeedDerivatives);
1338  theData.emplace_back(std::move(aData));
1339  nData++;
1340  }
1341  }
1342  }
1343  measDataIndex[numAllPoints + 1] = nData;
1344  // external measurements
1345  unsigned int nExt = externalMeasurements.rows();
1346  if (nExt > 0) {
1347  std::vector<unsigned int> index(numCurvature);
1348  std::vector<double> derivatives(numCurvature);
1349  for (unsigned int iExt = 0; iExt < nExt; ++iExt) {
1350  for (unsigned int iCol = 0; iCol < numCurvature; ++iCol) {
1351  index[iCol] = numLocals + iCol + 1;
1352  derivatives[iCol] = externalDerivatives(iExt, iCol);
1353  }
1354  GblData aData(1U, ExternalMeasurement, externalMeasurements(iExt),
1355  externalPrecisions(iExt));
1356  aData.addDerivatives(index, derivatives);
1357  theData.emplace_back(std::move(aData));
1358  nData++;
1359  }
1360  }
1361  measDataIndex[numAllPoints + 2] = nData;
1362 }
1363 
1365 void GblTrajectory::predict() {
1366  std::vector<GblData>::iterator itData;
1367  for (itData = theData.begin(); itData < theData.end(); ++itData) {
1368  itData->setPrediction(theVector);
1369  }
1370 }
1371 
1373 
1376 double GblTrajectory::downWeight(unsigned int aMethod) {
1377  double aLoss = 0.;
1378  std::vector<GblData>::iterator itData;
1379  for (itData = theData.begin(); itData < theData.end(); ++itData) {
1380  aLoss += (1. - itData->setDownWeighting(aMethod));
1381  }
1382  return aLoss;
1383 }
1384 
1386 
1398 unsigned int GblTrajectory::fit(double &Chi2, int &Ndf, double &lostWeight,
1399  const std::string& optionList, unsigned int aLabel) {
1400  const double normChi2[4] = { 1.0, 0.8737, 0.9326, 0.8228 };
1401  const std::string methodList = "TtHhCc";
1402 
1403  Chi2 = 0.;
1404  Ndf = -1;
1405  lostWeight = 0.;
1406  if (not constructOK)
1407  return 10;
1408 
1409  unsigned int aMethod = 0;
1410  skippedMeasLabel = aLabel;
1411 
1412  buildLinearEquationSystem();
1413  lostWeight = 0.;
1414  unsigned int ierr = 0;
1415  try {
1416 
1417  theMatrix.solveAndInvertBorderedBand(theVector, theVector);
1418  predict();
1419 
1420  for (unsigned int i = 0; i < optionList.size(); ++i) // down weighting iterations
1421  {
1422  size_t aPosition = methodList.find(optionList[i]);
1423  if (aPosition != std::string::npos) {
1424  aMethod = aPosition / 2 + 1;
1425  lostWeight = downWeight(aMethod);
1426  buildLinearEquationSystem();
1427  theMatrix.solveAndInvertBorderedBand(theVector, theVector);
1428  predict();
1429  }
1430  }
1431  Ndf = -numParameters;
1432  Chi2 = 0.;
1433  for (unsigned int i = 0; i < theData.size(); ++i) {
1434  // skipped (internal) measurement ?
1435  if (theData[i].getLabel() == skippedMeasLabel
1436  && theData[i].getType() == InternalMeasurement)
1437  continue;
1438  Chi2 += theData[i].getChi2();
1439  Ndf++;
1440  }
1441  Chi2 /= normChi2[aMethod];
1442  fitOK = true;
1443 
1444  } catch (int e) {
1445  std::cout << " GblTrajectory::fit exception " << e << std::endl;
1446  ierr = e;
1447  }
1448  return ierr;
1449 }
1450 
1452 
1455 void GblTrajectory::milleOut(MilleBinary &aMille) {
1456  double aValue;
1457  double aErr;
1458  unsigned int aTraj;
1459  unsigned int aPoint;
1460  unsigned int aRow;
1461  unsigned int numLocal;
1462  unsigned int* labLocal;
1463  double* derLocal;
1464  std::vector<int> labGlobal;
1465  std::vector<double> derGlobal;
1466 
1467  if (not constructOK)
1468  return;
1469 
1470  // data: measurements, kinks and external seed
1471  labGlobal.reserve(maxNumGlobals);
1472  derGlobal.reserve(maxNumGlobals);
1473  std::vector<GblData>::iterator itData;
1474  for (itData = theData.begin(); itData != theData.end(); ++itData) {
1475  itData->getAllData(aValue, aErr, numLocal, labLocal, derLocal, aTraj,
1476  aPoint, aRow);
1477  if (itData->getType() == InternalMeasurement)
1478  thePoints[aTraj][aPoint].getGlobalLabelsAndDerivatives(aRow,
1479  labGlobal, derGlobal);
1480  else
1481  labGlobal.resize(0);
1482  aMille.addData(aValue, aErr, numLocal, labLocal, derLocal, labGlobal,
1483  derGlobal);
1484  }
1485  aMille.writeRecord();
1486 }
1487 
1489 
1492 void GblTrajectory::printTrajectory(unsigned int level) const {
1493  if (numInnerTransformations) {
1494  std::cout << "Composed GblTrajectory, " << numInnerTransformations
1495  << " subtrajectories, type " << numInnerTransOffsets
1496  << std::endl;
1497  } else {
1498  std::cout << "Simple GblTrajectory" << std::endl;
1499  }
1500  if (theDimension.size() < 2) {
1501  std::cout << " 2D-trajectory" << std::endl;
1502  }
1503  std::cout << " Number of GblPoints : " << numAllPoints
1504  << std::endl;
1505  std::cout << " Number of points with offsets: " << numOffsets << std::endl;
1506  std::cout << " Number of fit parameters : " << numParameters
1507  << std::endl;
1508  std::cout << " Number of measurements : " << numMeasurements
1509  << std::endl;
1510  if (externalMeasurements.rows()) {
1511  std::cout << " Number of ext. measurements : "
1512  << externalMeasurements.rows() << std::endl;
1513  }
1514  if (externalPoint) {
1515  std::cout << " Label of point with ext. seed: " << externalPoint
1516  << std::endl;
1517  }
1518  if (constructOK) {
1519  std::cout << " Constructed OK " << std::endl;
1520  }
1521  if (fitOK) {
1522  std::cout << " Fitted OK " << std::endl;
1523  }
1524  if (level > 0) {
1525  IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
1526  if (numInnerTransformations) {
1527  std::cout << " Inner transformations" << std::endl;
1528  for (unsigned int i = 0; i < numInnerTransformations; ++i) {
1529  std::cout << innerTransformations[i].format(CleanFmt)
1530  << std::endl;
1531  }
1532  }
1533  if (externalMeasurements.rows()) {
1534  std::cout << " External measurements" << std::endl;
1535  std::cout << " Measurements:" << std::endl;
1536  std::cout << externalMeasurements.format(CleanFmt) << std::endl;
1537  std::cout << " Precisions:" << std::endl;
1538  std::cout << externalPrecisions.format(CleanFmt) << std::endl;
1539  std::cout << " Derivatives:" << std::endl;
1540  std::cout << externalDerivatives.format(CleanFmt) << std::endl;
1541  }
1542  if (externalPoint) {
1543  std::cout << " External seed:" << std::endl;
1544  std::cout << externalSeed.format(CleanFmt) << std::endl;
1545  }
1546  if (fitOK) {
1547  std::cout << " Fit results" << std::endl;
1548  std::cout << " Parameters:" << std::endl;
1549  theVector.print();
1550  std::cout << " Covariance matrix (bordered band part):"
1551  << std::endl;
1552  theMatrix.printMatrix();
1553  }
1554  }
1555 }
1556 
1558 
1561 void GblTrajectory::printPoints(unsigned int level) const {
1562  std::cout << "GblPoints " << std::endl;
1563  for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
1564  std::vector<GblPoint>::const_iterator itPoint;
1565  for (itPoint = thePoints[iTraj].begin();
1566  itPoint < thePoints[iTraj].end(); ++itPoint) {
1567  itPoint->printPoint(level);
1568  }
1569  }
1570 }
1571 
1573 void GblTrajectory::printData() const {
1574  std::cout << "GblData blocks " << std::endl;
1575  std::vector<GblData>::const_iterator itData;
1576  for (itData = theData.begin(); itData < theData.end(); ++itData) {
1577  itData->printData();
1578  }
1579 }
1580 
1581 }
gbl::BorderedBandMatrix::printMatrix
void printMatrix() const
Print bordered band matrix.
Definition: BorderedBandMatrix.cpp:243
gbl::BorderedBandMatrix::getBlockMatrix
Eigen::MatrixXd getBlockMatrix(const std::vector< unsigned int > anIndex) const
Retrieve symmetric block matrix.
Definition: BorderedBandMatrix.cpp:131
gbl::BorderedBandMatrix::resize
void resize(unsigned int nSize, unsigned int nBorder=1, unsigned int nBand=5)
Resize bordered band matrix.
Definition: BorderedBandMatrix.cpp:50
gbl::BorderedBandMatrix::solveAndInvertBorderedBand
void solveAndInvertBorderedBand(const VVector &aRightHandSide, VVector &aSolution)
Solve linear equation system, partially calculate inverse.
Definition: BorderedBandMatrix.cpp:210
gbl::BorderedBandMatrix::addBlockMatrix
void addBlockMatrix(double aWeight, const std::vector< unsigned int > *anIndex, const std::vector< double > *aVector)
Add symmetric block matrix.
Definition: BorderedBandMatrix.cpp:70
gbl::GblData
Data (block) for independent scalar measurement.
Definition: GblData.h:58
gbl::GblData::addDerivatives
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:144
gbl::GblPoint
Point on trajectory.
Definition: GblPoint.h:69
gbl::GblPoint::getDerivatives
void getDerivatives(int aDirection, Eigen::Matrix2d &matW, Eigen::Matrix2d &matWJ, Eigen::Vector2d &vecWd) const
Retrieve derivatives of local track model.
Definition: GblPoint.cpp:474
gbl::GblPoint::addNextJacobian
void addNextJacobian(const Matrix5d &aJac)
Define jacobian to next scatterer (by GBLTrajectory constructor)
Definition: GblPoint.cpp:460
gbl::GblPoint::getP2pJacobian
const Matrix5d & getP2pJacobian() const
Retrieve point-to-(previous)point jacobian.
Definition: GblPoint.cpp:437
gbl::GblPoint::getOffset
int getOffset() const
Retrieve offset for point.
Definition: GblPoint.cpp:432
gbl::GblTrajectory::numPoints
std::vector< unsigned int > numPoints
Number of points on (sub)trajectory.
Definition: GblTrajectory.h:165
gbl::GblTrajectory::scatDataIndex
std::vector< unsigned int > scatDataIndex
mapping points to data blocks from scatterers
Definition: GblTrajectory.h:183
gbl::GblTrajectory::theMatrix
BorderedBandMatrix theMatrix
(Bordered band) matrix of linear equation system
Definition: GblTrajectory.h:194
gbl::GblTrajectory::thePoints
std::vector< std::vector< GblPoint > > thePoints
(list of) List of points on trajectory
Definition: GblTrajectory.h:180
gbl::GblTrajectory::printPoints
void printPoints(unsigned int level=0) const
Print GblPoints on trajectory.
Definition: GblTrajectory.cpp:1561
gbl::GblTrajectory::skippedMeasLabel
unsigned int skippedMeasLabel
Label of point with measurements skipped in fit (for unbiased residuals) (or 0)
Definition: GblTrajectory.h:175
gbl::GblTrajectory::numTrajectories
unsigned int numTrajectories
Number of trajectories (in composed trajectory)
Definition: GblTrajectory.h:166
gbl::GblTrajectory::construct
void construct()
Construct trajectory from list of points.
Definition: GblTrajectory.cpp:424
gbl::GblTrajectory::externalPoint
unsigned int externalPoint
Label of external point (or 0)
Definition: GblTrajectory.h:174
gbl::GblTrajectory::externalSeed
Eigen::MatrixXd externalSeed
Precision (inverse covariance matrix) of external seed.
Definition: GblTrajectory.h:184
gbl::GblTrajectory::getMeasResults
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.
Definition: GblTrajectory.cpp:835
gbl::GblTrajectory::numAllPoints
unsigned int numAllPoints
Number of all points on trajectory.
Definition: GblTrajectory.h:164
gbl::GblTrajectory::downWeight
double downWeight(unsigned int aMethod)
Down-weight all points.
Definition: GblTrajectory.cpp:1376
gbl::GblTrajectory::theVector
VVector theVector
Vector of linear equation system.
Definition: GblTrajectory.h:193
gbl::GblTrajectory::calcJacobians
void calcJacobians()
Calculate Jacobians to previous/next scatterer from point to point ones.
Definition: GblTrajectory.cpp:494
gbl::GblTrajectory::numCurvature
unsigned int numCurvature
Number of curvature parameters (0 or 1) or external parameters.
Definition: GblTrajectory.h:170
gbl::GblTrajectory::externalPrecisions
Eigen::VectorXd externalPrecisions
Precisions for external measurements of composed trajectory.
Definition: GblTrajectory.h:191
gbl::GblTrajectory::innerTransLab
std::vector< std::array< unsigned int, 5 > > innerTransLab
Labels at innermost points of composed trajectory.
Definition: GblTrajectory.h:188
gbl::GblTrajectory::externalMeasurements
Eigen::VectorXd externalMeasurements
Residuals for external measurements of composed trajectory.
Definition: GblTrajectory.h:190
gbl::GblTrajectory::printTrajectory
void printTrajectory(unsigned int level=0) const
Print GblTrajectory.
Definition: GblTrajectory.cpp:1492
gbl::GblTrajectory::predict
void predict()
Calculate predictions for all points.
Definition: GblTrajectory.cpp:1365
gbl::GblTrajectory::defineOffsets
void defineOffsets()
Define offsets from list of points.
Definition: GblTrajectory.cpp:472
gbl::GblTrajectory::innerTransformations
std::vector< Eigen::MatrixXd > innerTransformations
Transformations at innermost points of composed trajectory (from common external parameters)
Definition: GblTrajectory.h:186
gbl::GblTrajectory::numLocals
unsigned int numLocals
Total number of (additional) local parameters.
Definition: GblTrajectory.h:172
gbl::GblTrajectory::getResAndErr
void getResAndErr(unsigned int aData, bool used, double &aResidual, double &aMeadsError, double &aResError, double &aDownWeight)
Get residual and errors from data block.
Definition: GblTrajectory.cpp:1036
gbl::GblTrajectory::milleOut
void milleOut(MilleBinary &aMille)
Write valid trajectory to Millepede-II binary file.
Definition: GblTrajectory.cpp:1455
gbl::GblTrajectory::getFitToLocalJacobian
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.
Definition: GblTrajectory.cpp:657
gbl::GblTrajectory::numInnerTransformations
unsigned int numInnerTransformations
Number of inner transformations to external parameters.
Definition: GblTrajectory.h:168
gbl::GblTrajectory::maxNumGlobals
unsigned int maxNumGlobals
Max. number of global labels/derivatives per point.
Definition: GblTrajectory.h:176
gbl::GblTrajectory::isValid
bool isValid() const
Retrieve validity of trajectory.
Definition: GblTrajectory.cpp:411
gbl::GblTrajectory::GblTrajectory
GblTrajectory(const std::vector< GblPoint > &aPointList, bool flagCurv=true, bool flagU1dir=true, bool flagU2dir=true)
Create new (simple) trajectory from list of points.
Definition: GblTrajectory.cpp:163
gbl::GblTrajectory::prepare
void prepare()
Prepare fit for simple or composed trajectory.
Definition: GblTrajectory.cpp:1093
gbl::GblTrajectory::getResults
unsigned int getResults(int aSignedLabel, Eigen::VectorXd &localPar, Eigen::MatrixXd &localCov) const
Get fit results at point.
Definition: GblTrajectory.cpp:804
gbl::GblTrajectory::externalDerivatives
Eigen::MatrixXd externalDerivatives
Derivatives for external measurements of composed trajectory.
Definition: GblTrajectory.h:189
gbl::GblTrajectory::fit
unsigned int fit(double &Chi2, int &Ndf, double &lostWeight, const std::string &optionList="", unsigned int aLabel=0)
Perform fit of (valid) trajectory.
Definition: GblTrajectory.cpp:1398
gbl::GblTrajectory::getScatResults
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.
Definition: GblTrajectory.cpp:865
gbl::GblTrajectory::theData
std::vector< GblData > theData
List of data blocks.
Definition: GblTrajectory.h:181
gbl::GblTrajectory::constructOK
bool constructOK
Trajectory has been successfully constructed (ready for fit/output)
Definition: GblTrajectory.h:177
gbl::GblTrajectory::numInnerTransOffsets
unsigned int numInnerTransOffsets
Number of (points with) offsets affected by inner transformations to external parameters.
Definition: GblTrajectory.h:169
gbl::GblTrajectory::measDataIndex
std::vector< unsigned int > measDataIndex
mapping points to data blocks from measurements
Definition: GblTrajectory.h:182
gbl::GblTrajectory::getFitToKinkJacobian
void getFitToKinkJacobian(std::array< unsigned int, 7 > &anIndex, Matrix27d &aJacobian, const GblPoint &aPoint) const
Get jacobian for transformation from (trajectory) fit to kink parameters at point.
Definition: GblTrajectory.cpp:753
gbl::GblTrajectory::buildLinearEquationSystem
void buildLinearEquationSystem()
Build linear equation system from data (blocks).
Definition: GblTrajectory.cpp:1061
gbl::GblTrajectory::printData
void printData() const
Print GblData blocks for trajectory.
Definition: GblTrajectory.cpp:1573
gbl::GblTrajectory::getLabels
unsigned int getLabels(std::vector< unsigned int > &aLabelList) const
Get (list of) labels of points on (simple) valid trajectory.
Definition: GblTrajectory.cpp:989
gbl::GblTrajectory::numParameters
unsigned int numParameters
Number of fit parameters.
Definition: GblTrajectory.h:171
gbl::GblTrajectory::numMeasurements
unsigned int numMeasurements
Total number of measurements.
Definition: GblTrajectory.h:173
gbl::GblTrajectory::innerTransDer
std::vector< Eigen::MatrixXd > innerTransDer
Derivatives at innermost points of composed trajectory.
Definition: GblTrajectory.h:187
gbl::GblTrajectory::getJacobian
std::pair< std::vector< unsigned int >, Eigen::MatrixXd > getJacobian(int aSignedLabel) const
Get jacobian for transformation from fit to track parameters at point.
Definition: GblTrajectory.cpp:540
gbl::GblTrajectory::fitOK
bool fitOK
Trajectory has been successfully fitted (results are valid)
Definition: GblTrajectory.h:178
gbl::GblTrajectory::numOffsets
unsigned int numOffsets
Number of (points with) offsets on trajectory.
Definition: GblTrajectory.h:167
gbl::GblTrajectory::theDimension
std::vector< unsigned int > theDimension
List of active dimensions (0=u1, 1=u2) in fit.
Definition: GblTrajectory.h:179
gbl::GblTrajectory::getNumPoints
unsigned int getNumPoints() const
Retrieve number of point from trajectory.
Definition: GblTrajectory.cpp:416
gbl::MilleBinary
Millepede-II (binary) record.
Definition: MilleBinary.h:81
gbl::MilleBinary::writeRecord
void writeRecord()
Write record to file.
Definition: MilleBinary.cpp:113
gbl::MilleBinary::addData
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:72
gbl::VVector::resize
void resize(const unsigned int nRows)
Resize vector.
Definition: VMatrix.cpp:262
gbl::VVector::print
void print() const
Print vector.
Definition: VMatrix.cpp:298
gbl
Namespace for the general broken lines package.
Definition: BorderedBandMatrix.cpp:34
gbl::Vector5d
Eigen::Matrix< double, 5, 1 > Vector5d
Definition: GblPoint.h:50
gbl::Matrix27d
Eigen::Matrix< double, 2, 7 > Matrix27d
Definition: GblData.h:47
gbl::InternalMeasurement
@ InternalMeasurement
Definition: GblData.h:50
gbl::ExternalMeasurement
@ ExternalMeasurement
Definition: GblData.h:50
gbl::ExternalSeed
@ ExternalSeed
Definition: GblData.h:50
gbl::InternalKink
@ InternalKink
Definition: GblData.h:50
gbl::Matrix5d
Eigen::Matrix< double, 5, 5 > Matrix5d
Definition: GblData.h:46
Generated on Thu Jul 29 2021 21:49:47 for GeneralBrokenLines by doxygen 1.9.1