src/TrackFitting/MbfSmoother.cpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/TrackFitting/MbfSmoother.hpp"
0010
0011 #include "Acts/EventData/AnyTrackStateProxy.hpp"
0012 #include "Acts/EventData/TrackParameterHelpers.hpp"
0013
0014 namespace Acts {
0015
0016 void MbfSmoother::calculateSmoothed(AnyMutableTrackStateProxy& ts,
0017                                     const BoundMatrix& bigLambdaHat,
0018                                     const BoundVector& smallLambdaHat) const {
0019   auto filteredCovariance = ts.filteredCovariance();
0020   auto smoothed = ts.smoothed();
0021   ts.smoothedCovariance() = filteredCovariance - filteredCovariance *
0022                                                      bigLambdaHat *
0023                                                      filteredCovariance;
0024   smoothed = ts.filtered() - filteredCovariance * smallLambdaHat;
0025   // Normalize phi and theta
0026   smoothed = normalizeBoundParameters(smoothed);
0027 }
0028
0029 void MbfSmoother::visitNonMeasurement(
0030     const AnyConstTrackStateProxy::ConstCovarianceMap& jacobian,
0031     BoundMatrix& bigLambdaHat, BoundVector& smallLambdaHat) const {
0032   const auto F = jacobian;
0033
0034   bigLambdaHat = F.transpose() * bigLambdaHat * F;
0035   smallLambdaHat = F.transpose() * smallLambdaHat;
0036 }
0037
0038 void MbfSmoother::visitMeasurement(const AnyConstTrackStateProxy& ts,
0039                                    BoundMatrix& bigLambdaHat,
0040                                    BoundVector& smallLambdaHat) const {
0041   assert(ts.hasCalibrated());
0042
0043   visit_measurement(
0044       ts.calibratedSize(),
0045       [&, this]<std::size_t N>(std::integral_constant<std::size_t, N>) {
0046         visitMeasurementImpl<N>(ts, bigLambdaHat, smallLambdaHat);
0047       });
0048 }
0049
0050 }  // namespace Acts