Analysis Software
Documentation for sPHENIX simulation software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GlobalChiSquareFitter.hpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file GlobalChiSquareFitter.hpp
1 // This file is part of the Acts project.
2 //
3 // Copyright (C) 2023 CERN for the benefit of the Acts project
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 
9 #pragma once
10 
11 // Workaround for building on clang+libstdc++
13 
41 
42 #include <functional>
43 #include <map>
44 #include <memory>
45 
46 namespace Acts {
47 namespace Experimental {
48 
50 template <typename traj_t>
53  using ConstTrackStateProxy =
55  using Parameters = typename TrackStateProxy::Parameters;
56 
57  using Calibrator =
58  Delegate<void(const GeometryContext&, const CalibrationContext&,
60 
62  Direction, const Logger&)>;
63 
65 
70 
73 
77 
80 
83  calibrator.template connect<&detail::voidFitterCalibrator<traj_t>>();
84  updater.template connect<&detail::voidFitterUpdater<traj_t>>();
85  outlierFinder.template connect<&detail::voidOutlierFinder<traj_t>>();
86  }
87 };
88 
92 template <typename traj_t>
107  const MagneticFieldContext& mctx,
108  std::reference_wrapper<const CalibrationContext> cctx,
109  Gx2FitterExtensions<traj_t> extensions_,
110  const PropagatorPlainOptions& pOptions,
111  const Surface* rSurface = nullptr, bool mScattering = false,
112  bool eLoss = false,
113  const FreeToBoundCorrection& freeToBoundCorrection_ =
114  FreeToBoundCorrection(false),
115  const size_t nUpdateMax_ = 5)
116  : geoContext(gctx),
117  magFieldContext(mctx),
118  calibrationContext(cctx),
119  extensions(extensions_),
120  propagatorPlainOptions(pOptions),
121  referenceSurface(rSurface),
122  multipleScattering(mScattering),
123  energyLoss(eLoss),
124  freeToBoundCorrection(freeToBoundCorrection_),
125  nUpdateMax(nUpdateMax_) {}
126 
128  Gx2FitterOptions() = delete;
129 
131  std::reference_wrapper<const GeometryContext> geoContext;
133  std::reference_wrapper<const MagneticFieldContext> magFieldContext;
135  std::reference_wrapper<const CalibrationContext> calibrationContext;
136 
138 
141 
143  const Surface* referenceSurface = nullptr;
144 
146  bool multipleScattering = false;
147 
149  bool energyLoss = false;
150 
154 
156  size_t nUpdateMax = 5;
157 };
158 
159 template <typename traj_t>
161  // Fitted states that the actor has handled.
162  traj_t* fittedStates{nullptr};
163 
164  // This is the index of the 'tip' of the track stored in multitrajectory.
165  // This corresponds to the last measurement state in the multitrajectory.
166  // Since this KF only stores one trajectory, it is unambiguous.
167  // SIZE_MAX is the start of a trajectory.
169 
170  // This is the index of the 'tip' of the states stored in multitrajectory.
171  // This corresponds to the last state in the multitrajectory.
172  // Since this KF only stores one trajectory, it is unambiguous.
173  // SIZE_MAX is the start of a trajectory.
175 
176  // The optional Parameters at the provided surface
177  std::optional<BoundTrackParameters> fittedParameters;
178 
179  // Counter for states with non-outlier measurements
180  size_t measurementStates = 0;
181 
182  // Counter for measurements holes
183  // A hole correspond to a surface with an associated detector element with no
184  // associated measurement. Holes are only taken into account if they are
185  // between the first and last measurements.
186  size_t measurementHoles = 0;
187 
188  // Counter for handled states
189  size_t processedStates = 0;
190 
191  // Indicator if track fitting has been done
192  bool finished = false;
193 
194  // Measurement surfaces without hits
195  std::vector<const Surface*> missedActiveSurfaces;
196 
197  // Measurement surfaces handled in both forward and
198  // backward filtering
199  std::vector<const Surface*> passedAgainSurfaces;
200 
202 
203  // collectors
204  std::vector<ActsScalar> collectorResiduals;
205  std::vector<ActsScalar> collectorCovariances;
206  std::vector<BoundVector> collectorProjectedJacobians;
207 
208  BoundMatrix jacobianFromStart = BoundMatrix::Identity();
209 
210  // Count how many surfaces have been hit
211  size_t surfaceCount = 0;
212 };
213 
230 template <size_t measDim, typename traj_t>
231 void collector(typename traj_t::TrackStateProxy& trackStateProxy,
232  Gx2FitterResult<traj_t>& result, const Logger& logger) {
233  auto predicted = trackStateProxy.predicted();
234  auto measurement = trackStateProxy.template calibrated<measDim>();
235  auto covarianceMeasurement =
236  trackStateProxy.template calibratedCovariance<measDim>();
237  // Project Jacobian and predicted measurements into the measurement dimensions
238  auto projJacobian = (trackStateProxy.projector()
239  .template topLeftCorner<measDim, eBoundSize>() *
240  result.jacobianFromStart)
241  .eval();
242  auto projPredicted = (trackStateProxy.projector()
243  .template topLeftCorner<measDim, eBoundSize>() *
244  predicted)
245  .eval();
246 
247  ACTS_VERBOSE("Processing and collecting measurements in Actor:\n"
248  << "\tMeasurement:\t" << measurement.transpose()
249  << "\n\tPredicted:\t" << predicted.transpose()
250  << "\n\tProjector:\t" << trackStateProxy.effectiveProjector()
251  << "\n\tProjected Jacobian:\t" << projJacobian
252  << "\n\tCovariance Measurements:\t" << covarianceMeasurement);
253 
254  // Collect residuals, covariances, and projected jacobians
255  for (size_t i = 0; i < measDim; i++) {
256  if (covarianceMeasurement(i, i) < 1e-10) {
257  ACTS_WARNING("Invalid covariance of measurement: cov(" << i << "," << i
258  << ") ~ 0")
259  continue;
260  }
261 
262  result.collectorResiduals.push_back(measurement[i] - projPredicted[i]);
263  result.collectorCovariances.push_back(covarianceMeasurement(i, i));
264  result.collectorProjectedJacobians.push_back(projJacobian.row(i));
265 
266  ACTS_VERBOSE("\tSplitting the measurement:\n"
267  << "\t\tResidual:\t" << measurement[i] - projPredicted[i]
268  << "\n\t\tCovariance:\t" << covarianceMeasurement(i, i)
269  << "\n\t\tProjected Jacobian:\t" << projJacobian.row(i));
270  }
271 }
272 
278 template <typename propagator_t, typename traj_t>
279 class Gx2Fitter {
281  using Gx2fNavigator = typename propagator_t::Navigator;
282 
284  static constexpr bool isDirectNavigator =
286 
287  public:
288  Gx2Fitter(propagator_t pPropagator,
289  std::unique_ptr<const Logger> _logger =
290  getDefaultLogger("Gx2Fitter", Logging::INFO))
291  : m_propagator(std::move(pPropagator)),
292  m_logger{std::move(_logger)},
293  m_actorLogger{m_logger->cloneWithSuffix("Actor")} {}
294 
295  private:
297  propagator_t m_propagator;
298 
300  std::unique_ptr<const Logger> m_logger;
301  std::unique_ptr<const Logger> m_actorLogger;
302 
303  const Logger& logger() const { return *m_logger; }
304 
313  template <typename parameters_t>
314  class Actor {
315  public:
318 
320  const Surface* targetSurface = nullptr;
321 
323  const std::map<GeometryIdentifier, SourceLink>* inputMeasurements = nullptr;
324 
326  bool multipleScattering = false;
327 
329  bool energyLoss = false;
330 
334 
336  std::shared_ptr<MultiTrajectory<traj_t>> outputStates;
337 
339  const Logger* actorLogger{nullptr};
340 
342  const Logger& logger() const { return *actorLogger; }
343 
345 
348 
351 
358 
369  template <typename propagator_state_t, typename stepper_t,
370  typename navigator_t>
371  void operator()(propagator_state_t& state, const stepper_t& stepper,
372  const navigator_t& navigator, result_type& result,
373  const Logger& /*logger*/) const {
374  assert(result.fittedStates && "No MultiTrajectory set");
375 
376  if (result.finished) {
377  return;
378  }
379 
380  // Add the measurement surface as external surface to navigator.
381  // We will try to hit those surface by ignoring boundary checks.
382  if constexpr (not isDirectNavigator) {
383  if (result.processedStates == 0) {
384  for (auto measurementIt = inputMeasurements->begin();
385  measurementIt != inputMeasurements->end(); measurementIt++) {
386  navigator.insertExternalSurface(state.navigation,
387  measurementIt->first);
388  }
389  }
390  }
391 
392  // Update:
393  // - Waiting for a current surface
394  auto surface = navigator.currentSurface(state.navigation);
395  // std::string direction = state.stepping.navDir.toString();
396  if (surface != nullptr) {
397  ++result.surfaceCount;
398  ACTS_VERBOSE("Measurement surface " << surface->geometryId()
399  << " detected.");
400 
401  // check if measurement surface
402  auto sourcelink_it = inputMeasurements->find(surface->geometryId());
403 
404  if (sourcelink_it != inputMeasurements->end()) {
405  ACTS_VERBOSE("Measurement surface " << surface->geometryId()
406  << " detected.");
407 
408  // Transport the covariance to the surface
409  stepper.transportCovarianceToBound(state.stepping, *surface,
411 
412  ACTS_VERBOSE(
413  "Actor - indices before processing:"
414  << "\n\t"
415  << "result.lastMeasurementIndex: " << result.lastMeasurementIndex
416  << "\n\t"
417  << "result.lastTrackIndex: " << result.lastTrackIndex << "\n\t"
418  << "result.fittedStates->size(): " << result.fittedStates->size())
419 
420  // TODO generalize the update of the currentTrackIndex
421  auto& fittedStates = *result.fittedStates;
422 
423  // Mask for the track states. We don't need Smoothed and Filtered
425  ~(TrackStatePropMask::Smoothed | TrackStatePropMask::Filtered);
426 
427  // Checks if an existing surface is found during the gx2f-iteration.
428  // If not, a new index will be generated afterwards.
429  // During the first iteration, we will always create a new index.
430  size_t currentTrackIndex = Acts::MultiTrajectoryTraits::kInvalid;
431  if (nUpdate == 0) {
432  ACTS_VERBOSE(" processSurface: nUpdate == 0 decision");
433 
434  // Add a <mask> TrackState entry multi trajectory. This allocates
435  // storage for all components, which we will set later.
436  currentTrackIndex =
437  fittedStates.addTrackState(mask, result.lastTrackIndex);
438  } else {
439  ACTS_VERBOSE(" processSurface: nUpdate > 0 decision");
440 
441  if (result.lastTrackIndex ==
443  currentTrackIndex = 0;
444  ACTS_VERBOSE(" processSurface: currentTrackIndex (kInv->0) = "
445  << currentTrackIndex);
446  } else if (result.lastTrackIndex < fittedStates.size() - 1) {
447  currentTrackIndex = result.lastTrackIndex + 1;
448  ACTS_VERBOSE(" processSurface: currentTrackIndex (n+1) = "
449  << currentTrackIndex);
450  } else {
451  // Add a <mask> TrackState entry multi trajectory. This allocates
452  // storage for all components, which we will set later.
453  currentTrackIndex =
454  fittedStates.addTrackState(mask, result.lastTrackIndex);
455  ACTS_VERBOSE(" processSurface: currentTrackIndex (ADD NEW)= "
456  << currentTrackIndex);
457  }
458  }
459 
460  // now get track state proxy back
461  typename traj_t::TrackStateProxy trackStateProxy =
462  fittedStates.getTrackState(currentTrackIndex);
463 
464  // Set the trackStateProxy components with the state from the ongoing
465  // propagation
466  {
467  trackStateProxy.setReferenceSurface(surface->getSharedPtr());
468  // Bind the transported state to the current surface
469  auto res = stepper.boundState(state.stepping, *surface, false,
471  if (!res.ok()) {
472  result.result = res.error();
473  return;
474  }
475  auto& [boundParams, jacobian, pathLength] = *res;
476 
477  // Fill the track state
478  trackStateProxy.predicted() = std::move(boundParams.parameters());
479  if (boundParams.covariance().has_value()) {
480  trackStateProxy.predictedCovariance() =
481  std::move(*boundParams.covariance());
482  }
483 
484  trackStateProxy.jacobian() = std::move(jacobian);
485  trackStateProxy.pathLength() = std::move(pathLength);
486  }
487 
488  // We have predicted parameters, so calibrate the uncalibrated input
489  // measurement
490  extensions.calibrator(state.geoContext, *calibrationContext,
491  sourcelink_it->second, trackStateProxy);
492 
493  // Get and set the type flags
494  auto typeFlags = trackStateProxy.typeFlags();
495  typeFlags.set(TrackStateFlag::ParameterFlag);
496  if (surface->surfaceMaterial() != nullptr) {
497  typeFlags.set(TrackStateFlag::MaterialFlag);
498  }
499 
500  result.jacobianFromStart =
501  trackStateProxy.jacobian() * result.jacobianFromStart;
502 
503  // Collect:
504  // - Residuals
505  // - Covariances
506  // - ProjectedJacobians
507  if (trackStateProxy.calibratedSize() == 1) {
508  collector<1>(trackStateProxy, result, *actorLogger);
509  } else if (trackStateProxy.calibratedSize() == 2) {
510  collector<2>(trackStateProxy, result, *actorLogger);
511  } else {
512  ACTS_WARNING(
513  "Only measurements of 1 and 2 dimensions are implemented yet.");
514  }
515 
516  // Set the measurement type flag
517  typeFlags.set(TrackStateFlag::MeasurementFlag);
518  // We count the processed state
519  ++result.processedStates;
520  ACTS_VERBOSE("Actor - indices after processing, before over writing:"
521  << "\n\t"
522  << "result.lastMeasurementIndex: "
523  << result.lastMeasurementIndex << "\n\t"
524  << "trackStateProxy.index(): " << trackStateProxy.index()
525  << "\n\t"
526  << "result.lastTrackIndex: " << result.lastTrackIndex
527  << "\n\t"
528  << "currentTrackIndex: " << currentTrackIndex)
529  result.lastMeasurementIndex = currentTrackIndex;
530  result.lastTrackIndex = currentTrackIndex;
531  } else {
532  ACTS_INFO("Actor: This case is not implemented yet")
533  }
534  }
535 
536  if (result.surfaceCount > 900) {
537  ACTS_INFO("Actor: finish due to limit. Result might be garbage.");
538  result.finished = true;
539  }
540  }
541  };
542 
544  template <typename parameters_t>
545  class Aborter {
546  public:
549 
550  template <typename propagator_state_t, typename stepper_t,
551  typename navigator_t, typename result_t>
552  bool operator()(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
553  const navigator_t& /*navigator*/, const result_t& result,
554  const Logger& /*logger*/) const {
555  if (!result.result.ok() or result.finished) {
556  return true;
557  }
558  return false;
559  }
560  };
561 
562  public:
581  template <typename source_link_iterator_t, typename start_parameters_t,
582  typename parameters_t = BoundTrackParameters,
583  typename track_container_t, template <typename> class holder_t,
584  bool _isdn = isDirectNavigator>
585  auto fit(source_link_iterator_t it, source_link_iterator_t end,
586  const start_parameters_t& sParameters,
587  const Gx2FitterOptions<traj_t>& gx2fOptions,
589  const -> std::enable_if_t<
590  !_isdn, Result<typename TrackContainer<track_container_t, traj_t,
591  holder_t>::TrackProxy>> {
592  // Preprocess Measurements (Sourcelinks -> map)
593  // To be able to find measurements later, we put them into a map
594  // We need to copy input SourceLinks anyway, so the map can own them.
595  ACTS_VERBOSE("Preparing " << std::distance(it, end)
596  << " input measurements");
597  std::map<GeometryIdentifier, SourceLink> inputMeasurements;
598 
599  for (; it != end; ++it) {
600  SourceLink sl = *it;
601  auto geoId = gx2fOptions.extensions.surfaceAccessor(sl)->geometryId();
602  inputMeasurements.emplace(geoId, std::move(sl));
603  }
604  ACTS_VERBOSE("inputMeasurements.size() = " << inputMeasurements.size());
605 
607  // Create the ActionList and AbortList
608  using GX2FAborter = Aborter<parameters_t>;
609  using GX2FActor = Actor<parameters_t>;
610 
611  using GX2FResult = typename GX2FActor::result_type;
612  using Actors = Acts::ActionList<GX2FActor>;
613  using Aborters = Acts::AbortList<GX2FAborter>;
614 
616 
617  const size_t reducedMatrixSize = 4;
618  start_parameters_t params = sParameters;
619  BoundVector deltaParams = BoundVector::Zero();
620  double chi2sum = 0;
621  BoundMatrix aMatrix = BoundMatrix::Zero();
622  BoundVector bVector = BoundVector::Zero();
623 
624  // Create a index of the 'tip' of the track stored in multitrajectory. It is
625  // needed outside the update loop. It will be updated with each iteration
626  // and used for the final track
627  size_t tipIndex = Acts::MultiTrajectoryTraits::kInvalid;
628 
629  ACTS_VERBOSE("params:\n" << params);
630 
632  ACTS_DEBUG("Start to iterate");
633 
634  // Iterate the fit and improve result. Abort after n steps or after
635  // convergence
636  for (size_t nUpdate = 0; nUpdate < gx2fOptions.nUpdateMax; nUpdate++) {
637  ACTS_VERBOSE("nUpdate = " << nUpdate + 1 << "/"
638  << gx2fOptions.nUpdateMax);
639 
640  // update params
641  params.parameters() += deltaParams;
642  ACTS_VERBOSE("updated params:\n" << params);
643 
644  // set up propagator and co
645  Acts::GeometryContext geoCtx = gx2fOptions.geoContext;
646  Acts::MagneticFieldContext magCtx = gx2fOptions.magFieldContext;
647  // Set options for propagator
648  PropagatorOptions propagatorOptions(geoCtx, magCtx);
649  auto& gx2fActor = propagatorOptions.actionList.template get<GX2FActor>();
650  gx2fActor.inputMeasurements = &inputMeasurements;
651  gx2fActor.extensions = gx2fOptions.extensions;
652  gx2fActor.calibrationContext = &gx2fOptions.calibrationContext.get();
653  gx2fActor.actorLogger = m_actorLogger.get();
654  gx2fActor.nUpdate = nUpdate;
655 
656  typename propagator_t::template action_list_t_result_t<
658  inputResult;
659 
660  auto& r = inputResult.template get<Gx2FitterResult<traj_t>>();
661 
662  r.fittedStates = &trackContainer.trackStateContainer();
663  // propagate with params and return jacobians and residuals
664  auto result = m_propagator.template propagate(
665  params, propagatorOptions, true, std::move(inputResult));
666 
667  // TODO Improve Propagator + Actor [allocate before loop], rewrite
668  // makeMeasurements
669  auto& propRes = *result;
670  GX2FResult gx2fResult = std::move(propRes.template get<GX2FResult>());
671 
672  ACTS_VERBOSE("gx2fResult.collectorResiduals.size() = "
673  << gx2fResult.collectorResiduals.size());
674  ACTS_VERBOSE("gx2fResult.collectorCovariances.size() = "
675  << gx2fResult.collectorCovariances.size());
676  ACTS_VERBOSE("gx2fResult.collectorProjectedJacobians.size() = "
677  << gx2fResult.collectorProjectedJacobians.size());
678 
679  chi2sum = 0;
680  aMatrix = BoundMatrix::Zero();
681  bVector = BoundVector::Zero();
682 
683  // TODO generalize for non-2D measurements
684  for (size_t iMeas = 0; iMeas < gx2fResult.collectorResiduals.size();
685  iMeas++) {
686  const auto ri = gx2fResult.collectorResiduals[iMeas];
687  const auto covi = gx2fResult.collectorCovariances[iMeas];
688  const auto projectedJacobian =
689  gx2fResult.collectorProjectedJacobians[iMeas];
690 
691  const double chi2meas = ri / covi * ri;
692  const BoundMatrix aMatrixMeas =
693  projectedJacobian * projectedJacobian.transpose() / covi;
694  const BoundVector bVectorMeas = projectedJacobian / covi * ri;
695 
696  chi2sum += chi2meas;
697  aMatrix += aMatrixMeas;
698  bVector += bVectorMeas;
699  }
700 
701  // calculate delta params [a] * delta = b
702  deltaParams = BoundVector::Zero();
703  const ActsVector<reducedMatrixSize> deltaParamsReduced =
704  aMatrix.topLeftCorner<reducedMatrixSize, reducedMatrixSize>()
705  .colPivHouseholderQr()
706  .solve(bVector.topLeftCorner<reducedMatrixSize, 1>());
707 
708  for (size_t idp = 0; idp < reducedMatrixSize; idp++) {
709  deltaParams(idp, 0) = deltaParamsReduced(idp, 0);
710  }
711 
712  ACTS_VERBOSE("chi2sum = " << chi2sum);
713  ACTS_VERBOSE("aMatrix:\n" << aMatrix);
714  ACTS_VERBOSE("bVector:\n" << bVector);
715  ACTS_VERBOSE("deltaParams:\n" << deltaParams);
716 
717  tipIndex = gx2fResult.lastMeasurementIndex;
718 
719  // TODO check delta params and abort
720  // similar to:
721  // if (sum(delta_params) < 1e-3) {
722  // break;
723  // }
724  }
725  ACTS_DEBUG("Finished to iterate");
726  ACTS_VERBOSE("final params:\n" << params);
728 
729  // Calculate covariance of the fitted parameters with inverse of [a]
730  BoundMatrix fullCovariancePredicted = BoundMatrix::Identity();
731  if (aMatrix.topLeftCorner<reducedMatrixSize, reducedMatrixSize>()
732  .determinant() != 0) {
733  fullCovariancePredicted
734  .template topLeftCorner<reducedMatrixSize, reducedMatrixSize>() =
735  aMatrix.topLeftCorner<reducedMatrixSize, reducedMatrixSize>()
736  .inverse();
737  } else if (gx2fOptions.nUpdateMax > 0) {
738  ACTS_ERROR("det(a) == 0. This should not happen ever.");
739  return Experimental::GlobalChiSquareFitterError::DetAIsZero;
740  }
741 
742  // Prepare track for return
743  auto track = trackContainer.getTrack(trackContainer.addTrack());
744  track.tipIndex() = tipIndex;
745  track.parameters() = params.parameters();
746  track.covariance() = fullCovariancePredicted;
747  track.setReferenceSurface(params.referenceSurface().getSharedPtr());
748 
749  // TODO write test for calculateTrackQuantities
751 
752  // Return the converted Track
753  return track;
754  }
755 };
756 } // namespace Experimental
757 } // namespace Acts