Skip to content

Commit

Permalink
refactor: Reduce abuse of auto in mbf smoother (#3630)
Browse files Browse the repository at this point in the history 
Reduce abuse of `auto`
  • Loading branch information
@CarloVarni
CarloVarni authored Sep 23, 2024
1 parent 59250ad commit ebde4f7
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 47 deletions.
28 changes: 13 additions & 15 deletions Core/include/Acts/TrackFitting/MbfSmoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,21 +51,21 @@ class MbfSmoother {

using TrackStateProxy = typename traj_t::TrackStateProxy;

TrackStateProxy start_ts = trajectory.getTrackState(entryIndex);
TrackStateProxy startTs = trajectory.getTrackState(entryIndex);

// Notation consistent with the Wikipedia article
// https://en.wikipedia.org/wiki/Kalman_filter
BoundMatrix big_lambda_hat = BoundMatrix::Zero();
BoundVector small_lambda_hat = BoundVector::Zero();
BoundMatrix bigLambdaHat = BoundMatrix::Zero();
BoundVector smallLambdaHat = BoundVector::Zero();

trajectory.applyBackwards(start_ts.index(), [&](TrackStateProxy ts) {
trajectory.applyBackwards(startTs.index(), [&](TrackStateProxy ts) {
// ensure the track state has a smoothed component
ts.addComponents(TrackStatePropMask::Smoothed);

InternalTrackState internalTrackState(ts);

// Smoothe the current state
calculateSmoothed(internalTrackState, big_lambda_hat, small_lambda_hat);
calculateSmoothed(internalTrackState, bigLambdaHat, smallLambdaHat);

// We smoothed the last state - no need to update the lambdas
if (!ts.hasPrevious()) {
Expand All @@ -74,10 +74,9 @@ class MbfSmoother {

// Update the lambdas depending on the type of track state
if (ts.typeFlags().test(TrackStateFlag::MeasurementFlag)) {
visitMeasurement(internalTrackState, big_lambda_hat, small_lambda_hat);
visitMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
} else {
visitNonMeasurement(internalTrackState, big_lambda_hat,
small_lambda_hat);
visitNonMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
}
});

Expand Down Expand Up @@ -143,18 +142,17 @@ class MbfSmoother {

/// Calculate the smoothed parameters and covariance.
void calculateSmoothed(InternalTrackState& ts,
const BoundMatrix& big_lambda_hat,
const BoundVector& small_lambda_hat) const;
const BoundMatrix& bigLambdaHat,
const BoundVector& smallLambdaHat) const;

/// Visit a non-measurement track state and update the lambdas.
void visitNonMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const;
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const;

/// Visit a measurement track state and update the lambdas.
void visitMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const;
void visitMeasurement(const InternalTrackState& ts, BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const;
};

} // namespace Acts
68 changes: 36 additions & 32 deletions Core/src/TrackFitting/MbfSmoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,68 +11,72 @@
namespace Acts {

void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
const BoundMatrix& big_lambda_hat,
const BoundVector& small_lambda_hat) const {
const BoundMatrix& bigLambdaHat,
const BoundVector& smallLambdaHat) const {
ts.smoothedCovariance = ts.filteredCovariance - ts.filteredCovariance *
big_lambda_hat *
bigLambdaHat *
ts.filteredCovariance;
ts.smoothed = ts.filtered - ts.filteredCovariance * small_lambda_hat;
ts.smoothed = ts.filtered - ts.filteredCovariance * smallLambdaHat;
}

void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const {
const auto F = ts.jacobian;
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const {
const InternalTrackState::Jacobian F = ts.jacobian;

big_lambda_hat = F.transpose() * big_lambda_hat * F;
small_lambda_hat = F.transpose() * small_lambda_hat;
bigLambdaHat = F.transpose() * bigLambdaHat * F;
smallLambdaHat = F.transpose() * smallLambdaHat;
}

void MbfSmoother::visitMeasurement(const InternalTrackState& ts,
BoundMatrix& big_lambda_hat,
BoundVector& small_lambda_hat) const {
BoundMatrix& bigLambdaHat,
BoundVector& smallLambdaHat) const {
assert(ts.measurement.has_value());

const auto& measurement = ts.measurement.value();
const InternalTrackState::Measurement& measurement = ts.measurement.value();
const InternalTrackState::Jacobian F = ts.jacobian;

visit_measurement(measurement.calibratedSize, [&](auto N) -> void {
constexpr std::size_t kMeasurementSize = decltype(N)::value;

using MeasurementMatrix =
Eigen::Matrix<ActsScalar, kMeasurementSize, eBoundSize>;
using CovarianceMatrix =
Eigen::Matrix<ActsScalar, kMeasurementSize, kMeasurementSize>;
using KalmanGainMatrix =
Eigen::Matrix<ActsScalar, eBoundSize, kMeasurementSize>;

typename TrackStateTraits<kMeasurementSize, true>::Calibrated calibrated{
measurement.calibrated};
typename TrackStateTraits<kMeasurementSize, true>::CalibratedCovariance
calibratedCovariance{measurement.calibratedCovariance};

// Measurement matrix
const auto H = measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>()
.eval();
const MeasurementMatrix H =
measurement.projector
.template topLeftCorner<kMeasurementSize, eBoundSize>();

// Residual covariance
const auto S =
(H * ts.predictedCovariance * H.transpose() + calibratedCovariance)
.eval();
const CovarianceMatrix S =
(H * ts.predictedCovariance * H.transpose() + calibratedCovariance);
// TODO Sinv could be cached by the filter step
const auto S_inv = S.inverse().eval();
const CovarianceMatrix SInv = S.inverse();

// Kalman gain
// TODO K could be cached by the filter step
const auto K = (ts.predictedCovariance * H.transpose() * S_inv).eval();

const auto C_hat = (BoundMatrix::Identity() - K * H).eval();
const auto y = (calibrated - H * ts.predicted).eval();
const KalmanGainMatrix K = (ts.predictedCovariance * H.transpose() * SInv);

const auto big_lambda_tilde =
(H.transpose() * S_inv * H + C_hat.transpose() * big_lambda_hat * C_hat)
.eval();
const auto small_lambda_tilde =
(-H.transpose() * S_inv * y + C_hat.transpose() * small_lambda_hat)
.eval();
const Acts::BoundMatrix CHat = (Acts::BoundMatrix::Identity() - K * H);
const Eigen::Matrix<ActsScalar, kMeasurementSize, 1> y =
(calibrated - H * ts.predicted);

const auto F = ts.jacobian;
const Acts::BoundMatrix bigLambdaTilde =
(H.transpose() * SInv * H + CHat.transpose() * bigLambdaHat * CHat);
const Eigen::Matrix<ActsScalar, eBoundSize, 1> smallLambdaTilde =
(-H.transpose() * SInv * y + CHat.transpose() * smallLambdaHat);

big_lambda_hat = F.transpose() * big_lambda_tilde * F;
small_lambda_hat = F.transpose() * small_lambda_tilde;
bigLambdaHat = F.transpose() * bigLambdaTilde * F;
smallLambdaHat = F.transpose() * smallLambdaTilde;
});
}

Expand Down

0 comments on commit ebde4f7

Please sign in to comment.