Belle II Software  release-05-02-19
CovarianceMatrixUtil.h
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * Copyright(C) 2016 - Belle II Collaboration *
4  * *
5  * Author: The Belle II Collaboration *
6  * Contributors: Oliver Frost *
7  * *
8  * This software is provided "as is" without any warranty. *
9  **************************************************************************/
10 #pragma once
11 
12 #include <tracking/trackFindingCDC/numerics/PrecisionMatrixUtil.h>
13 #include <tracking/trackFindingCDC/numerics/JacobianMatrixUtil.h>
14 
15 #include <tracking/trackFindingCDC/numerics/CovarianceMatrix.h>
16 #include <tracking/trackFindingCDC/numerics/PrecisionMatrix.h>
17 #include <tracking/trackFindingCDC/numerics/JacobianMatrix.h>
18 #include <tracking/trackFindingCDC/numerics/ParameterVector.h>
19 
20 #include <tracking/trackFindingCDC/numerics/EigenView.h>
21 
22 #include <Eigen/Core>
23 #include <Eigen/LU>
24 #include <Eigen/QR>
25 
26 #include <type_traits>
27 #include <cmath>
28 #include <cassert>
29 
30 namespace Belle2 {
35  namespace TrackFindingCDC {
36 
38  struct CovarianceMatrixUtil {
39 
41  template <int N>
42  static CovarianceMatrix<N> identity()
43  {
45  }
46 
48  template <int N>
49  static void transport(const JacobianMatrix<N, N>& jacobian, CovarianceMatrix<N>& cov)
50  {
51  mapToEigen(cov) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
52  }
53 
55  template <int M, int N>
57  const CovarianceMatrix<N>& cov)
58  {
59  CovarianceMatrix<M> result;
60  mapToEigen(result) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
61  return result;
62  }
63 
65  template <int N>
66  static void scale(const ParameterVector<N>& scales, CovarianceMatrix<N>& cov)
67  {
68  return transport(JacobianMatrixUtil::scale(scales), cov);
69  }
70 
72  template <int N>
73  static CovarianceMatrix<N> scale(const ParameterVector<N>& scales,
74  const CovarianceMatrix<N>& cov)
75  {
76  return transported(JacobianMatrixUtil::scale(scales), cov);
77  }
78 
81  template <int N1, int N2>
83  const CovarianceMatrix<N2>& block2)
84  {
86  mapToEigen(result) << mapToEigen(block1), Eigen::Matrix<double, N1, N2>::Zero(),
87  Eigen::Matrix<double, N2, N1>::Zero(), mapToEigen(block2);
88  return result;
89  }
90 
92  template <class ACovarianceMatrix, int I = 0, int N = 0>
93  static ACovarianceMatrix getSub(const CovarianceMatrix<N>& cov)
94  {
95  constexpr const int M =
96  std::remove_reference_t<decltype(mapToEigen(ACovarianceMatrix()))>::RowsAtCompileTime;
97  return cov.template block<M, M>(I, I);
98  }
99 
101  template <int N>
103  {
105  mapToEigen(cov) = mapToEigen(prec).colPivHouseholderQr().inverse();
106  return cov;
107  }
108 
110  template <int N>
112  {
113  PrecisionMatrix<N> prec;
114  mapToEigen(prec) = mapToEigen(cov).colPivHouseholderQr().inverse();
115  return prec;
116  }
117 
119  template <int N>
121  {
123  mapToEigen(cov) = mapToEigen(prec).inverse();
124  return cov;
125  }
126 
128  template <int N>
130  {
131  PrecisionMatrix<N> prec;
132  mapToEigen(prec) = mapToEigen(cov).inverse();
133  return prec;
134  }
135 
147  template <int N>
148  static double average(const ParameterVector<N>& par1,
149  const CovarianceMatrix<N>& cov1,
150  const ParameterVector<N>& par2,
151  const CovarianceMatrix<N>& cov2,
152  ParameterVector<N>& par,
153  CovarianceMatrix<N>& cov)
154  {
155  PrecisionMatrix<N> precision1 = toPrecision(cov1);
156  PrecisionMatrix<N> precision2 = toPrecision(cov2);
157  PrecisionMatrix<N> precision;
158  double chi2 =
159  PrecisionMatrixUtil::average(par1, precision1, par2, precision2, par, precision);
160  cov = fromPrecision(precision);
161  return chi2;
162  }
163 
178  template <int M, int N1>
179  static double average(const ParameterVector<N1>& par1,
180  const CovarianceMatrix<N1>& cov1,
181  const JacobianMatrix<N1, M>& ambiguity1,
182  const ParameterVector<M>& par2,
183  const CovarianceMatrix<M>& cov2,
184  ParameterVector<M>& par,
185  CovarianceMatrix<M>& cov)
186  {
187  /* Naive implementation */
188  JacobianMatrix<M, M> ambiguity2 = identity<M>();
189  return average(par1, cov1, ambiguity1, par2, cov2, ambiguity2, par, cov);
190 
191  /* Kalman implementation - may be faster */
192  // par = par2;
193  // cov = cov2;
194  // return kalmanUpdate(par1, cov1, ambiguity1, par, cov);
195  }
196 
211  template <int M, int N1, int N2>
212  static double average(const ParameterVector<N1>& par1,
213  const CovarianceMatrix<N1>& cov1,
214  const JacobianMatrix<N1, M>& ambiguity1,
215  const ParameterVector<N2>& par2,
216  const CovarianceMatrix<N2>& cov2,
217  const JacobianMatrix<N2, M>& ambiguity2,
218  ParameterVector<M>& par,
219  CovarianceMatrix<M>& cov)
220  {
221  PrecisionMatrix<N1> precision1 = toPrecision(cov1);
222  PrecisionMatrix<N2> precision2 = toPrecision(cov2);
223  PrecisionMatrix<M> precision;
224  double chi2 = PrecisionMatrixUtil::average(par1,
225  precision1,
226  ambiguity1,
227  par2,
228  precision2,
229  ambiguity2,
230  par,
231  precision);
232  cov = fromPrecision(precision);
233  return chi2;
234  }
235 
250  template <int M, int N1>
251  static double kalmanUpdate(const ParameterVector<N1>& par1,
252  const CovarianceMatrix<N1>& cov1,
253  const JacobianMatrix<N1, M>& ambiguity1,
254  ParameterVector<M>& par2,
255  CovarianceMatrix<M>& cov2)
256  {
257  /* Kalman update - smart version */
258  const auto& ePar1 = mapToEigen(par1);
259  const auto& eCov1 = mapToEigen(cov1);
260  const auto& eAmbiguity1 = mapToEigen(ambiguity1);
261 
262  auto&& ePar2 = mapToEigen(par2);
263  auto&& eCov2 = mapToEigen(cov2);
264 
265  // Apriori residual
266  auto residual = ePar1 - eAmbiguity1 * ePar2;
267  auto residualCov = eCov1 + eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
268  auto residualPrecision = residualCov.inverse();
269 
270  // Chi2 - calculate on the apriori residuals, which is the same value as for the posterior.
271  Eigen::Matrix<double, 1, 1> chi2 = residual.transpose() * residualPrecision * residual;
272 
273  // Update parameters
274  auto kalmanGain = eCov2 * eAmbiguity1.transpose() * residualPrecision;
275  eCov2 -= kalmanGain * eAmbiguity1 * eCov2;
276  ePar2 += kalmanGain * residual;
277 
278  return chi2[0];
279 
280  // Posterior residual - here for reference, should be compiled out.
281  auto postResidual = ePar1 - eAmbiguity1 * ePar2;
282  auto postResidualCov = eCov1 - eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
283  auto postResidualPrecision = postResidualCov.inverse();
284  Eigen::Matrix<double, 1, 1> postChi2 =
285  postResidual.transpose() * postResidualPrecision * postResidual;
286  (void)postChi2;
287  assert(std::fabs(postChi2[0] - chi2[0]) < 1e-8);
288  }
289  };
290 
291  }
293 }
Belle2::TrackFindingCDC::CovarianceMatrixUtil::scale
static void scale(const ParameterVector< N > &scales, CovarianceMatrix< N > &cov)
Scale the covariance inplace by the given factors in each parameter.
Definition: CovarianceMatrixUtil.h:74
Belle2::TrackFindingCDC::CovarianceMatrixUtil::transported
static CovarianceMatrix< M > transported(const JacobianMatrix< M, N > &jacobian, const CovarianceMatrix< N > &cov)
Return a copy of the covariance matrix transported with the given jacobian matrix.
Definition: CovarianceMatrixUtil.h:64
Belle2::TrackFindingCDC::JacobianMatrixUtil::scale
static JacobianMatrix< N > scale(const ParameterVector< N > &scales)
Calculates the jacobian matrix for a scaling in each parameter.
Definition: JacobianMatrixUtil.h:76
Belle2::TrackFindingCDC::CovarianceMatrixUtil::toPrecision
static PrecisionMatrix< N > toPrecision(const CovarianceMatrix< N > &cov)
Convert covariance matrix to precision matrix - allows for reduced rank.
Definition: CovarianceMatrixUtil.h:119
Belle2::TrackFindingCDC::CovarianceMatrixUtil::stackBlocks
static CovarianceMatrix< N1+N2 > stackBlocks(const CovarianceMatrix< N1 > &block1, const CovarianceMatrix< N2 > &block2)
Combines two covariance matrices by putting them in two blocks on the diagonal of a larger matrix.
Definition: CovarianceMatrixUtil.h:90
Belle2::TrackFindingCDC::CovarianceMatrixUtil::average
static double average(const ParameterVector< N > &par1, const CovarianceMatrix< N > &cov1, const ParameterVector< N > &par2, const CovarianceMatrix< N > &cov2, ParameterVector< N > &par, CovarianceMatrix< N > &cov)
Averages two parameter vectors taking into account their respective covariances.
Definition: CovarianceMatrixUtil.h:156
Belle2::TrackFindingCDC::CovarianceMatrixUtil::identity
static CovarianceMatrix< N > identity()
Constructs an identity matrix.
Definition: CovarianceMatrixUtil.h:50
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19
Belle2::TrackFindingCDC::CovarianceMatrixUtil::fullToPrecision
static PrecisionMatrix< N > fullToPrecision(const CovarianceMatrix< N > &cov)
Convert precision matrix to covariance matrix - assumes full rank.
Definition: CovarianceMatrixUtil.h:137
Belle2::TrackFindingCDC::CovarianceMatrixUtil::getSub
static ACovarianceMatrix getSub(const CovarianceMatrix< N > &cov)
Gets a subcovariance from a covariance matrix.
Definition: CovarianceMatrixUtil.h:101
Belle2::TrackFindingCDC::CovarianceMatrixUtil::transport
static void transport(const JacobianMatrix< N, N > &jacobian, CovarianceMatrix< N > &cov)
Transport the covariance matrix inplace with the given jacobian matrix.
Definition: CovarianceMatrixUtil.h:57
Belle2::TrackFindingCDC::CovarianceMatrixUtil::fromPrecision
static CovarianceMatrix< N > fromPrecision(const PrecisionMatrix< N > &prec)
Convert precision matrix to covariance matrix - allows for reduced rank.
Definition: CovarianceMatrixUtil.h:110
Belle2::TrackFindingCDC::CovarianceMatrixUtil::fromFullPrecision
static CovarianceMatrix< N > fromFullPrecision(const PrecisionMatrix< N > &prec)
Convert covariance matrix to precision matrix - assumes full rank.
Definition: CovarianceMatrixUtil.h:128
Belle2::TrackFindingCDC::PlainMatrix
A matrix implementation to be used as an interface typ through out the track finder.
Definition: PlainMatrix.h:50
Belle2::TrackFindingCDC::CovarianceMatrixUtil::kalmanUpdate
static double kalmanUpdate(const ParameterVector< N1 > &par1, const CovarianceMatrix< N1 > &cov1, const JacobianMatrix< N1, M > &ambiguity1, ParameterVector< M > &par2, CovarianceMatrix< M > &cov2)
Updates a parameter and its covariance by integrating information from parameter vector in a projecte...
Definition: CovarianceMatrixUtil.h:259
Belle2::TrackFindingCDC::PrecisionMatrixUtil::average
static double average(const ParameterVector< N > &parameter1, const PrecisionMatrix< N > &precision1, const ParameterVector< N > &parameter2, const PrecisionMatrix< N > &precision2, ParameterVector< N > &parameter, PrecisionMatrix< N > &precision)
Averages two parameter vectors taking into account their respective precision.
Definition: PrecisionMatrixUtil.h:114
Belle2::TrackFindingCDC::PlainMatrix::Identity
static PlainMatrix< T, M, N > Identity()
Construct an identity matrix.
Definition: PlainMatrix.h:84