CMS 3D CMS Logo
SurveyPxbImageLocalFit.cc
Go to the documentation of this file.
1 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImage.h"
2 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImageLocalFit.h"
3 
4 #include <stdexcept>
5 #include <utility>
6 #include <sstream>
7 #include <vector>
8 #include <cmath>
9 #include "DataFormats/GeometryVector/interface/LocalPoint.h"
10 #include "Math/SMatrix.h"
11 #include "Math/SVector.h"
12 
13 #include <iostream>
14 
15 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec,
16  const pede_label_t &label1,
17  const pede_label_t &label2) {
18  labelVec1_.clear();
19  labelVec1_.push_back(label1 + 0);
20  labelVec1_.push_back(label1 + 1);
21  labelVec1_.push_back(label1 + 5);
22  labelVec2_.clear();
23  labelVec2_.push_back(label2 + 0);
24  labelVec2_.push_back(label2 + 1);
25  labelVec2_.push_back(label2 + 5);
26  doFit(fidpointvec);
27 }
28 
29 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec) {
30  fitValidFlag_ = false;
31 
32  // Calculate gamma of right module w.r.t left modules' fram
33  const value_t dxr = fidpointvec[3].x() - fidpointvec[2].x();
34  const value_t dyr = fidpointvec[3].y() - fidpointvec[2].y();
35  const value_t gammar = atan(dyr / dxr);
36  const value_t dxl = fidpointvec[1].x() - fidpointvec[0].x();
37  const value_t dyl = fidpointvec[1].y() - fidpointvec[0].y();
38  const value_t gammal = atan(dyl / dxl);
39  const value_t gamma = gammar - gammal;
40  //const value_t gamma = 0.; // Testhalber
41  const value_t sing = sin(gamma);
42  const value_t cosg = cos(gamma);
43 #ifdef DEBUG
44  std::cout << "gamma: " << gamma << " gamma left: " << gammal << " gamma right: " << gammar << std::endl;
45 #endif
46 
47 #ifdef DEBUG
48  std::cout << "&fidpointvec: " << std::endl;
49  for (count_t i = 0; i != fidpointvec.size(); i++)
50  std::cout << i << ": " << fidpointvec[i] << std::endl;
51  std::cout << "&fidpoints_: " << std::endl;
52  for (count_t i = 0; i != fidpoints_.size(); i++)
53  std::cout << i << ": " << fidpoints_[i] << std::endl;
54 #endif
55 
56  // Matrix of the local derivatives
57  ROOT::Math::SMatrix<value_t, nMsrmts, nLcD> A; // 8x4
58  A(0, 0) = 1.;
59  A(0, 1) = 0;
60  A(0, 2) = +fidpointvec[0].x();
61  A(0, 3) = +fidpointvec[0].y();
62  A(1, 0) = 0.;
63  A(1, 1) = 1;
64  A(1, 2) = +fidpointvec[0].y();
65  A(1, 3) = -fidpointvec[0].x();
66  A(2, 0) = 1.;
67  A(2, 1) = 0;
68  A(2, 2) = +fidpointvec[1].x();
69  A(2, 3) = +fidpointvec[1].y();
70  A(3, 0) = 0.;
71  A(3, 1) = 1;
72  A(3, 2) = +fidpointvec[1].y();
73  A(3, 3) = -fidpointvec[1].x();
74  A(4, 0) = 1.;
75  A(4, 1) = 0;
76  A(4, 2) = +fidpointvec[2].x();
77  A(4, 3) = +fidpointvec[2].y();
78  A(5, 0) = 0.;
79  A(5, 1) = 1;
80  A(5, 2) = +fidpointvec[2].y();
81  A(5, 3) = -fidpointvec[2].x();
82  A(6, 0) = 1.;
83  A(6, 1) = 0;
84  A(6, 2) = +fidpointvec[3].x();
85  A(6, 3) = +fidpointvec[3].y();
86  A(7, 0) = 0.;
87  A(7, 1) = 1;
88  A(7, 2) = +fidpointvec[3].y();
89  A(7, 3) = -fidpointvec[3].x();
90 #ifdef DEBUG
91  std::cout << "A: \n" << A << std::endl;
92 #endif
93 
94  // Covariance matrix
95  ROOT::Math::SMatrix<value_t, nMsrmts, nMsrmts> W; // 8x8
96  //ROOT::Math::MatRepSym<value_t,8> W;
97  const value_t sigma_x2inv = 1. / (sigma_x_ * sigma_x_);
98  const value_t sigma_y2inv = 1. / (sigma_y_ * sigma_y_);
99  W(0, 0) = sigma_x2inv;
100  W(1, 1) = sigma_y2inv;
101  W(2, 2) = sigma_x2inv;
102  W(3, 3) = sigma_y2inv;
103  W(4, 4) = sigma_x2inv;
104  W(5, 5) = sigma_y2inv;
105  W(6, 6) = sigma_x2inv;
106  W(7, 7) = sigma_y2inv;
107 #ifdef DEBUG
108  std::cout << "W: \n" << W << std::endl;
109 #endif
110 
111  // Prepare for the fit
112  ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWA; // 4x4
113  ATWA = ROOT::Math::Transpose(A) * W * A;
114  //ATWA = ROOT::Math::SimilarityT(A,W); // W muss symmterisch sein -> aendern.
115  //std::cout << "ATWA: \n" << ATWA << std::endl;
116  ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWAi; // 4x4
117  int ifail = 0;
118  ATWAi = ATWA.Inverse(ifail);
119  if (ifail != 0) { // TODO: ifail Pruefung auf message logger ausgeben statt cout
120  std::cout << "Problem singular - fit impossible." << std::endl;
121  fitValidFlag_ = false;
122  return;
123  }
124 #ifdef DEBUG
125  std::cout << "ATWA-1: \n" << ATWAi << ifail << std::endl;
126 #endif
127 
128  // Measurements
129  ROOT::Math::SVector<value_t, nMsrmts> y; // 8
130  y(0) = measurementVec_[0].x();
131  y(1) = measurementVec_[0].y();
132  y(2) = measurementVec_[1].x();
133  y(3) = measurementVec_[1].y();
134  y(4) = measurementVec_[2].x();
135  y(5) = measurementVec_[2].y();
136  y(6) = measurementVec_[3].x();
137  y(7) = measurementVec_[3].y();
138 #ifdef DEBUG
139  std::cout << "y: " << y << std::endl;
140 #endif
141 
142  // do the fit
143  ROOT::Math::SVector<value_t, nLcD> a; // 4
144  a = ATWAi * ROOT::Math::Transpose(A) * W * y;
145  chi2_ = ROOT::Math::Dot(y, W * y) - ROOT::Math::Dot(a, ROOT::Math::Transpose(A) * W * y);
146 #ifdef DEBUG
147  std::cout << "a: " << a << " S= " << sqrt(a[2] * a[2] + a[3] * a[3]) << " phi= " << atan(a[3] / a[2])
148  << " chi2= " << chi2_ << std::endl;
149  std::cout << "A*a: " << A * a << std::endl;
150 #endif
151  a_.assign(a.begin(), a.end());
152 
153  // Calculate vector of residuals
154  r = y - A * a;
155 #ifdef DEBUG
156  std::cout << "r: " << r << std::endl;
157 #endif
158 
159  // Fill matrix for global fit with local derivatives
160  localDerivsMatrix_(0, 0) = 1.;
161  localDerivsMatrix_(0, 1) = 0;
162  localDerivsMatrix_(1, 0) = 0.;
163  localDerivsMatrix_(1, 1) = 1;
164  localDerivsMatrix_(2, 0) = 1.;
165  localDerivsMatrix_(2, 1) = 0;
166  localDerivsMatrix_(3, 0) = 0.;
167  localDerivsMatrix_(3, 1) = 1;
168  localDerivsMatrix_(4, 0) = 1.;
169  localDerivsMatrix_(4, 1) = 0;
170  localDerivsMatrix_(5, 0) = 0.;
171  localDerivsMatrix_(5, 1) = 1;
172  localDerivsMatrix_(6, 0) = 1.;
173  localDerivsMatrix_(6, 1) = 0;
174  localDerivsMatrix_(7, 0) = 0.;
175  localDerivsMatrix_(7, 1) = 1;
176  localDerivsMatrix_(0, 2) = +fidpointvec[0].x() + cosg * fidpoints_[0].x() - sing * fidpoints_[0].y();
177  localDerivsMatrix_(0, 3) = +fidpointvec[0].y() + cosg * fidpoints_[0].y() + sing * fidpoints_[0].x();
178  localDerivsMatrix_(1, 2) = +fidpointvec[0].y() + cosg * fidpoints_[0].y() + sing * fidpoints_[0].x();
179  localDerivsMatrix_(1, 3) = -fidpointvec[0].x() - cosg * fidpoints_[0].x() + sing * fidpoints_[0].y();
180  localDerivsMatrix_(2, 2) = +fidpointvec[1].x() + cosg * fidpoints_[1].x() - sing * fidpoints_[1].y();
181  localDerivsMatrix_(2, 3) = +fidpointvec[1].y() + cosg * fidpoints_[1].y() + sing * fidpoints_[1].x();
182  localDerivsMatrix_(3, 2) = +fidpointvec[1].y() + cosg * fidpoints_[1].y() + sing * fidpoints_[1].x();
183  localDerivsMatrix_(3, 3) = -fidpointvec[1].x() - cosg * fidpoints_[1].x() + sing * fidpoints_[1].y();
184  localDerivsMatrix_(4, 2) = +fidpointvec[2].x() + cosg * fidpoints_[2].x() - sing * fidpoints_[2].y();
185  localDerivsMatrix_(4, 3) = +fidpointvec[2].y() + cosg * fidpoints_[2].y() + sing * fidpoints_[2].x();
186  localDerivsMatrix_(5, 2) = +fidpointvec[2].y() + cosg * fidpoints_[2].y() + sing * fidpoints_[2].x();
187  localDerivsMatrix_(5, 3) = -fidpointvec[2].x() - cosg * fidpoints_[2].x() + sing * fidpoints_[2].y();
188  localDerivsMatrix_(6, 2) = +fidpointvec[3].x() + cosg * fidpoints_[3].x() - sing * fidpoints_[3].y();
189  localDerivsMatrix_(6, 3) = +fidpointvec[3].y() + cosg * fidpoints_[3].y() + sing * fidpoints_[3].x();
190  localDerivsMatrix_(7, 2) = +fidpointvec[3].y() + cosg * fidpoints_[3].y() + sing * fidpoints_[3].x();
191  localDerivsMatrix_(7, 3) = -fidpointvec[3].x() - cosg * fidpoints_[3].x() + sing * fidpoints_[3].y();
192 
193  // Fill vector with global derivatives and labels (8x3)
194  globalDerivsMatrix_(0, 0) = +a(2);
195  globalDerivsMatrix_(0, 1) = +a(3);
196  globalDerivsMatrix_(0, 2) = +cosg * (a(3) * fidpoints_[0].x() - a(2) * fidpoints_[0].y()) -
197  sing * (a(2) * fidpoints_[0].x() + a(3) * fidpoints_[0].y());
198  globalDerivsMatrix_(1, 0) = -a(3);
199  globalDerivsMatrix_(1, 1) = +a(2);
200  globalDerivsMatrix_(1, 2) = +cosg * (a(2) * fidpoints_[0].x() + a(3) * fidpoints_[0].y()) -
201  sing * (a(2) * fidpoints_[0].y() - a(3) * fidpoints_[0].x());
202  globalDerivsMatrix_(2, 0) = +a(2);
203  globalDerivsMatrix_(2, 1) = +a(3);
204  globalDerivsMatrix_(2, 2) = +cosg * (a(3) * fidpoints_[1].x() - a(2) * fidpoints_[1].y()) -
205  sing * (a(2) * fidpoints_[1].x() + a(3) * fidpoints_[1].y());
206  globalDerivsMatrix_(3, 0) = -a(3);
207  globalDerivsMatrix_(3, 1) = +a(2);
208  globalDerivsMatrix_(3, 2) = +cosg * (a(2) * fidpoints_[1].x() + a(3) * fidpoints_[1].y()) -
209  sing * (a(2) * fidpoints_[1].y() - a(3) * fidpoints_[1].x());
210 
211  globalDerivsMatrix_(4, 0) = +a(2);
212  globalDerivsMatrix_(4, 1) = +a(3);
213  globalDerivsMatrix_(4, 2) = +cosg * (a(3) * fidpoints_[2].x() - a(2) * fidpoints_[2].y()) -
214  sing * (a(2) * fidpoints_[2].x() + a(3) * fidpoints_[2].y());
215  globalDerivsMatrix_(5, 0) = -a(3);
216  globalDerivsMatrix_(5, 1) = +a(2);
217  globalDerivsMatrix_(5, 2) = +cosg * (a(2) * fidpoints_[2].x() + a(3) * fidpoints_[2].y()) -
218  sing * (a(2) * fidpoints_[2].y() - a(3) * fidpoints_[2].x());
219  globalDerivsMatrix_(6, 0) = +a(2);
220  globalDerivsMatrix_(6, 1) = +a(3);
221  globalDerivsMatrix_(6, 2) = +cosg * (a(3) * fidpoints_[3].x() - a(2) * fidpoints_[3].y()) -
222  sing * (a(2) * fidpoints_[3].x() + a(3) * fidpoints_[3].y());
223  globalDerivsMatrix_(7, 0) = -a(3);
224  globalDerivsMatrix_(7, 1) = +a(2);
225  globalDerivsMatrix_(7, 2) = +cosg * (a(2) * fidpoints_[3].x() + a(3) * fidpoints_[3].y()) -
226  sing * (a(2) * fidpoints_[3].y() - a(3) * fidpoints_[3].x());
227 
228  fitValidFlag_ = true;
229 }
230 
231 void SurveyPxbImageLocalFit::doFit(value_t u1, value_t v1, value_t g1, value_t u2, value_t v2, value_t g2) {
232  // Creating vectors with the global parameters of the two modules
233  ROOT::Math::SVector<value_t, 4> mod1, mod2;
234  mod1(0) = u1;
235  mod1(1) = v1;
236  mod1(2) = cos(g1);
237  mod1(3) = sin(g1);
238  mod2(0) = u2;
239  mod2(1) = v2;
240  mod2(2) = cos(g2);
241  mod2(3) = sin(g2);
242  //std::cout << "mod1: " << mod1 << std::endl;
243  //std::cout << "mod2: " << mod2 << std::endl;
244 
245  // Create a matrix for the transformed position of the fidpoints
246  ROOT::Math::SMatrix<value_t, 4, 4> M1, M2;
247  M1(0, 0) = 1.;
248  M1(0, 1) = 0.;
249  M1(0, 2) = +fidpoints_[0].x();
250  M1(0, 3) = -fidpoints_[0].y();
251  M1(1, 0) = 0.;
252  M1(1, 1) = 1.;
253  M1(1, 2) = +fidpoints_[0].y();
254  M1(1, 3) = +fidpoints_[0].x();
255  M1(2, 0) = 1.;
256  M1(2, 1) = 0.;
257  M1(2, 2) = +fidpoints_[1].x();
258  M1(2, 3) = -fidpoints_[1].y();
259  M1(3, 0) = 0.;
260  M1(3, 1) = 1.;
261  M1(3, 2) = +fidpoints_[1].y();
262  M1(3, 3) = +fidpoints_[1].x();
263  M2(0, 0) = 1.;
264  M2(0, 1) = 0.;
265  M2(0, 2) = +fidpoints_[2].x();
266  M2(0, 3) = -fidpoints_[2].y();
267  M2(1, 0) = 0.;
268  M2(1, 1) = 1.;
269  M2(1, 2) = +fidpoints_[2].y();
270  M2(1, 3) = +fidpoints_[2].x();
271  M2(2, 0) = 1.;
272  M2(2, 1) = 0.;
273  M2(2, 2) = +fidpoints_[3].x();
274  M2(2, 3) = -fidpoints_[3].y();
275  M2(3, 0) = 0.;
276  M2(3, 1) = 1.;
277  M2(3, 2) = +fidpoints_[3].y();
278  M2(3, 3) = +fidpoints_[3].x();
279 
280  //std::cout << "M1:\n" << M1 << std::endl;
281  //std::cout << "M2:\n" << M2 << std::endl;
282 
283  ROOT::Math::SVector<value_t, 4> mod_tr1, mod_tr2;
284  mod_tr1 = M1 * mod2;
285  mod_tr2 = M2 * mod1;
286  //std::cout << "mod_tr1: " << mod_tr1 << std::endl;
287  //std::cout << "mod_tr2: " << mod_tr2 << std::endl;
288 
289  fidpoint_t fidpointvec;
290  fidpointvec.push_back(coord_t(mod_tr1(0), mod_tr1(1)));
291  fidpointvec.push_back(coord_t(mod_tr1(2), mod_tr1(3)));
292  fidpointvec.push_back(coord_t(mod_tr2(0), mod_tr2(1)));
293  fidpointvec.push_back(coord_t(mod_tr2(2), mod_tr2(3)));
294 
295  doFit(fidpointvec);
296 }
297 
298 SurveyPxbImageLocalFit::localpars_t SurveyPxbImageLocalFit::getLocalParameters() {
299  if (!fitValidFlag_)
300  throw std::logic_error(
301  "SurveyPxbImageLocalFit::getLocalParameters(): Fit is not valid. Call doFit(...) before calling this "
302  "function.");
303  return a_;
304 }
305 
306 SurveyPxbImageLocalFit::value_t SurveyPxbImageLocalFit::getChi2() {
307  if (!fitValidFlag_)
308  throw std::logic_error(
309  "SurveyPxbImageLocalFit::getChi2(): Fit is not valid. Call doFit(...) before calling this function.");
310  return chi2_;
311 }
312 
313 void SurveyPxbImageLocalFit::setLocalDerivsToZero(count_t j) {
314  if (!(j < nLcD))
315  throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
316  for (count_t i = 0; i != nMsrmts; i++)
317  localDerivsMatrix_(i, j) = 0;
318 }
319 
320 void SurveyPxbImageLocalFit::setGlobalDerivsToZero(count_t j) {
321  if (!(j < nGlD))
322  throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
323  for (count_t i = 0; i != nMsrmts; i++)
324  globalDerivsMatrix_(i, j) = 0;
325 }
SurveyPxbImage::sigma_x_
value_t sigma_x_
Gaussian errors.
Definition: SurveyPxbImage.h:81
SurveyPxbImage::coord_t
Point3DBase< value_t, LocalTag > coord_t
Definition: SurveyPxbImage.h:17
DDAxes::y
SurveyPxbImage::sigma_y_
value_t sigma_y_
Definition: SurveyPxbImage.h:81
mps_fire.i
i
Definition: mps_fire.py:355
SurveyPxbImageLocalFit::r
ROOT::Math::SVector< value_t, nMsrmts > r
Vector of residuals.
Definition: SurveyPxbImageLocalFit.h:70
count_t
Definition: ErrorPropogationTypes.h:7
SurveyPxbImageLocalFit::fidpoints_
std::vector< coord_t > fidpoints_
True position of the fiducial points on a sensor wrt. local frame (u,v)
Definition: SurveyPxbImageLocalFit.h:84
SurveyPxbImageLocalFit::localDerivsMatrix_
ROOT::Math::SMatrix< pede_deriv_t, nMsrmts, nLcD > localDerivsMatrix_
Matrix with local derivs.
Definition: SurveyPxbImageLocalFit.h:78
SurveyPxbImage.h
gather_cfg.cout
cout
Definition: gather_cfg.py:144
diffTwoXMLs.g1
g1
Definition: diffTwoXMLs.py:52
SurveyPxbImage::measurementVec_
std::vector< coord_t > measurementVec_
Vector to hold four measurements.
Definition: SurveyPxbImage.h:74
CustomPhysics_cfi.gamma
gamma
Definition: CustomPhysics_cfi.py:17
funct::sin
Sin< T >::type sin(const T &t)
Definition: Sin.h:22
SurveyPxbImageLocalFit::doFit
void doFit(const fidpoint_t &fidpointvec)
Invoke the fit.
Definition: SurveyPxbImageLocalFit.cc:29
funct::cos
Cos< T >::type cos(const T &t)
Definition: Cos.h:22
SurveyPxbImageLocalFit::fidpoint_t
std::vector< coord_t > fidpoint_t
Definition: SurveyPxbImageLocalFit.h:17
bTagCommon_cff.label2
label2
Definition: bTagCommon_cff.py:168
mathSSE::sqrt
T sqrt(T t)
Definition: SSEVec.h:19
SurveyPxbImageLocalFit::setLocalDerivsToZero
void setLocalDerivsToZero(count_t i)
Definition: SurveyPxbImageLocalFit.cc:313
SurveyPxbImageLocalFit.h
a
double a
Definition: hdecay.h:119
SurveyPxbImageLocalFit::globalDerivsMatrix_
ROOT::Math::SMatrix< pede_deriv_t, nMsrmts, nGlD > globalDerivsMatrix_
Matrix with global derivs.
Definition: SurveyPxbImageLocalFit.h:74
SurveyPxbImageLocalFit::nLcD
static const count_t nLcD
Definition: SurveyPxbImageLocalFit.h:20
SurveyPxbImageLocalFit::pede_label_t
int pede_label_t
Definition: SurveyPxbImageLocalFit.h:25
A
testProducerWithPsetDescEmpty_cfi.u1
u1
Definition: testProducerWithPsetDescEmpty_cfi.py:49
SurveyPxbImageLocalFit::a_
localpars_t a_
Local parameters.
Definition: SurveyPxbImageLocalFit.h:67
SurveyPxbImageLocalFit::setGlobalDerivsToZero
void setGlobalDerivsToZero(count_t i)
Definition: SurveyPxbImageLocalFit.cc:320
diffTwoXMLs.g2
g2
Definition: diffTwoXMLs.py:71
SurveyPxbImageLocalFit::chi2_
value_t chi2_
chi2 of the local fit
Definition: SurveyPxbImageLocalFit.h:87
MaterialEffects_cfi.A
A
Definition: MaterialEffects_cfi.py:11
SurveyPxbImageLocalFit::nGlD
static const count_t nGlD
Definition: SurveyPxbImageLocalFit.h:19
SurveyPxbImageLocalFit::getChi2
value_t getChi2()
returns the chi^2 of the fit
Definition: SurveyPxbImageLocalFit.cc:306
bTagCommon_cff.label1
label1
Definition: bTagCommon_cff.py:167
LocalPoint.h
SurveyPxbImage::value_t
double value_t
Definition: SurveyPxbImage.h:16
SurveyPxbImageLocalFit::getLocalParameters
localpars_t getLocalParameters()
returns local parameters after fit
Definition: SurveyPxbImageLocalFit.cc:298
SurveyPxbImageLocalFit::labelVec2_
std::vector< pede_label_t > labelVec2_
Definition: SurveyPxbImageLocalFit.h:81
SurveyPxbImageLocalFit::fitValidFlag_
bool fitValidFlag_
Validity Flag.
Definition: SurveyPxbImageLocalFit.h:90
SurveyPxbImageLocalFit::labelVec1_
std::vector< pede_label_t > labelVec1_
Vector with labels to global derivs.
Definition: SurveyPxbImageLocalFit.h:81
dqmiolumiharvest.j
j
Definition: dqmiolumiharvest.py:66
MetAnalyzer.u2
u2
Definition: MetAnalyzer.py:61
SurveyPxbImageLocalFit::nMsrmts
static const count_t nMsrmts
Definition: SurveyPxbImageLocalFit.h:21
SurveyPxbImageLocalFit::localpars_t
std::vector< value_t > localpars_t
Definition: SurveyPxbImageLocalFit.h:16
Generated for CMSSW Reference Manual by doxygen 1.8.16