Belle II Software  release-05-01-25
EigenObservationMatrix.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * Copyright(C) 2017 - 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 #include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
11 
12 #include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
13 #include <tracking/trackFindingCDC/fitting/CDCSZObservations.h>
14 
15 #include <cassert>
16 
17 using namespace Belle2;
18 using namespace TrackFindingCDC;
19 
20 TrackFindingCDC::EigenObservationMatrix
21 TrackFindingCDC::getEigenObservationMatrix(CDCObservations2D* observations2D)
22 {
23  std::size_t nObservations = observations2D->size();
24  double* rawObservations = observations2D->data();
25  EigenObservationMatrix eigenObservations(rawObservations, nObservations, 4);
26  return eigenObservations;
27 }
28 
29 TrackFindingCDC::EigenObservationMatrix
30 TrackFindingCDC::getEigenObservationMatrix(CDCSZObservations* szObservations)
31 {
32  std::size_t nObservations = szObservations->size();
33  double* rawObservations = szObservations->data();
34  EigenObservationMatrix eigenObservations(rawObservations, nObservations, 3);
35  return eigenObservations;
36 }
37 
38 Eigen::Matrix<double, 5, 5> TrackFindingCDC::getWXYRLSumMatrix(CDCObservations2D& observations2D)
39 {
40  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
41  assert(eigenObservations.cols() == 4);
42  std::size_t nObservations = eigenObservations.rows();
43 
44  // B2INFO("Matrix of observations: " << endl << eigenObservations);
45 
46  Eigen::Matrix<double, Eigen::Dynamic, 5> projectedPoints(nObservations, 5);
47 
48  const std::size_t iW = 0;
49  const std::size_t iX = 1;
50  const std::size_t iY = 2;
51  const std::size_t iR2 = 3;
52  const std::size_t iL = 4;
53 
54  projectedPoints.col(iW) =
55  Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0); // Offset column
56  projectedPoints.col(iX) = eigenObservations.col(0);
57  projectedPoints.col(iY) = eigenObservations.col(1);
58  projectedPoints.col(iR2) = eigenObservations.leftCols<2>().rowwise().squaredNorm() -
59  eigenObservations.col(2).rowwise().squaredNorm();
60  projectedPoints.col(iL) = eigenObservations.col(2);
61 
62  Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
63  Eigen::Matrix<double, Eigen::Dynamic, 5> weightedProjectedPoints =
64  projectedPoints.array().colwise() * weights;
65  Eigen::Matrix<double, 5, 5> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
66 
67  // B2INFO("Matrix of sums: " << endl << sumMatrix);
68 
69  return sumMatrix;
70 }
71 
72 Eigen::Matrix<double, 4, 4> TrackFindingCDC::getWXYLSumMatrix(CDCObservations2D& observations2D)
73 {
74  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
75  assert(eigenObservations.cols() == 4);
76  std::size_t nObservations = eigenObservations.rows();
77 
78  Eigen::Matrix<double, Eigen::Dynamic, 4> projectedPoints(nObservations, 4);
79 
80  const std::size_t iW = 0;
81  const std::size_t iX = 1;
82  const std::size_t iY = 2;
83  const std::size_t iL = 3;
84 
85  projectedPoints.col(iW) =
86  Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0); // Offset column
87  projectedPoints.col(iX) = eigenObservations.col(0);
88  projectedPoints.col(iY) = eigenObservations.col(1);
89  projectedPoints.col(iL) = eigenObservations.col(2);
90 
91  Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
92  Eigen::Matrix<double, Eigen::Dynamic, 4> weightedProjectedPoints =
93  projectedPoints.array().colwise() * weights;
94  Eigen::Matrix<double, 4, 4> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
95 
96  return sumMatrix;
97 }
98 
99 Eigen::Matrix<double, 4, 4> TrackFindingCDC::getWXYRSumMatrix(CDCObservations2D& observations2D)
100 {
101  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
102  assert(eigenObservations.cols() == 4);
103  std::size_t nObservations = eigenObservations.rows();
104 
105  Eigen::Matrix<double, Eigen::Dynamic, 4> projectedPoints(nObservations, 4);
106 
107  const std::size_t iW = 0;
108  const std::size_t iX = 1;
109  const std::size_t iY = 2;
110  const std::size_t iR2 = 3;
111 
112  projectedPoints.col(iW) =
113  Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0); // Offset column
114  projectedPoints.col(iX) = eigenObservations.col(0);
115  projectedPoints.col(iY) = eigenObservations.col(1);
116  projectedPoints.col(iR2) = eigenObservations.leftCols<2>().rowwise().squaredNorm() -
117  eigenObservations.col(2).rowwise().squaredNorm();
118 
119  Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
120  Eigen::Matrix<double, Eigen::Dynamic, 4> weightedProjectedPoints =
121  projectedPoints.array().colwise() * weights;
122  Eigen::Matrix<double, 4, 4> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
123  return sumMatrix;
124 }
125 
126 Eigen::Matrix<double, 3, 3> TrackFindingCDC::getWXYSumMatrix(CDCObservations2D& observations2D)
127 {
128  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
129  assert(eigenObservations.cols() == 4);
130  std::size_t nObservations = eigenObservations.rows();
131 
132  Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
133 
134  const std::size_t iW = 0;
135  const std::size_t iX = 1;
136  const std::size_t iY = 2;
137 
138  projectedPoints.col(iW) =
139  Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0); // Offset column
140  projectedPoints.col(iX) = eigenObservations.col(0);
141  projectedPoints.col(iY) = eigenObservations.col(1);
142 
143  Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
144  Eigen::Matrix<double, Eigen::Dynamic, 3> weightedProjectedPoints =
145  projectedPoints.array().colwise() * weights;
146  Eigen::Matrix<double, 3, 3> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
147 
148  return sumMatrix;
149 }
150 
151 Eigen::Matrix<double, 3, 3> TrackFindingCDC::getWSZSumMatrix(CDCSZObservations& szObservations)
152 {
153  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&szObservations);
154  assert(eigenObservations.cols() == 3);
155  std::size_t nObservations = eigenObservations.rows();
156 
157  Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
158 
159  const std::size_t iW = 0;
160  const std::size_t iS = 1;
161  const std::size_t iZ = 2;
162 
163  projectedPoints.col(iW) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0); // Offset column
164  projectedPoints.col(iS) = eigenObservations.col(0);
165  projectedPoints.col(iZ) = eigenObservations.col(1);
166 
167  Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(2);
168  Eigen::Matrix<double, Eigen::Dynamic, 3> weightedProjectedPoints = projectedPoints.array().colwise() * weights;
169  Eigen::Matrix<double, 3, 3> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
170 
171  return sumMatrix;
172 }
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19