ATLAS Offline Software
Loading...
Searching...
No Matches
VtCFitE.cxx
Go to the documentation of this file.
1/*
2 Copyright (C) 2002-2023 CERN for the benefit of the ATLAS collaboration
3*/
4
11#include <cmath>
12
13namespace Trk {
14
15/* ---------------------------------------------------------- */
16/* Entry for error matrix calculation after successful fit */
17/* Error matrix has a form V(X,Y,Z,PX,PY,PZ) */
18/* ADER - full covariance matrix after fit in form */
19/* (x,y,z,track1(1:3),track2(1:3),......) */
20
21#define ader_ref(a_1,a_2) ader[(a_2)*(vkalNTrkM*3+3) + (a_1) - (vkalNTrkM*3+4)]
22#define dcv_ref(a_1,a_2) dcv[(a_2)*6 + (a_1) - 7]
23#define useWeightScheme 1
24
25int getFullVrtCov(VKVertex * vk, double *ader, const double *dcv, double verr[6][6])
26{
27
28 int i,j,ic1,ic2;
29
30 long int ic, jc, it, jt;
31 double cnt = 1e8;
32
33 TWRK * t_trk=nullptr;
34 long int NTRK = vk->TrackList.size();
35 long int IERR=0;
36 long int NVar = (NTRK + 1) * 3;
37 if(vk->passNearVertex && vk->ConstraintList.empty()) {
38 /* Fit is with "pass near" constraint and then */
39 /* matrix is already present */
40 } else if ( !vk->ConstraintList.empty() && useWeightScheme ) {
41/* Full matrix inversion i */
42//
43 FullMTXfill( vk, ader);
44 if ( vk->passNearVertex ) {
45 double drdpy[2][3];
46 double dpipj[3][3];
47 for (it = 1; it <= NTRK; ++it) {
48 drdpy[0][0] = vk->tmpArr[it-1]->drdp[0][0] * vk->FVC.ywgt[0] + vk->tmpArr[it-1]->drdp[1][0] * vk->FVC.ywgt[1];
49 drdpy[1][0] = vk->tmpArr[it-1]->drdp[0][0] * vk->FVC.ywgt[1] + vk->tmpArr[it-1]->drdp[1][0] * vk->FVC.ywgt[2];
50 drdpy[0][1] = vk->tmpArr[it-1]->drdp[0][1] * vk->FVC.ywgt[0] + vk->tmpArr[it-1]->drdp[1][1] * vk->FVC.ywgt[1];
51 drdpy[1][1] = vk->tmpArr[it-1]->drdp[0][1] * vk->FVC.ywgt[1] + vk->tmpArr[it-1]->drdp[1][1] * vk->FVC.ywgt[2];
52 drdpy[0][2] = vk->tmpArr[it-1]->drdp[0][2] * vk->FVC.ywgt[0] + vk->tmpArr[it-1]->drdp[1][2] * vk->FVC.ywgt[1];
53 drdpy[1][2] = vk->tmpArr[it-1]->drdp[0][2] * vk->FVC.ywgt[1] + vk->tmpArr[it-1]->drdp[1][2] * vk->FVC.ywgt[2];
54 for (jt = 1; jt <= NTRK; ++jt) { /* Matrix */
55 for (int k = 0; k < 3; ++k) {
56 for (int l = 0; l < 3; ++l) {
57 dpipj[k][l] = 0.;
58 for (int j = 0; j < 2; ++j) {
59 dpipj[k][l] += vk->tmpArr[jt-1]->drdp[j][k] * drdpy[j][l];
60 }
61 }
62 }
63 for (int k = 1; k <= 3; ++k) {
64 for (int l = 1; l <= 3; ++l) {
65 ader_ref(it * 3 + k, jt * 3 + l) += dpipj[l-1][k-1];
66 }
67 }
68 }
69 }
70 }
71 Vect3DF th0t,tf0t;
72 for(ic1=0; ic1<(int)vk->ConstraintList.size();ic1++){
73 for(ic2=0; ic2<vk->ConstraintList[ic1]->NCDim; ic2++){
74 th0t = vk->ConstraintList[ic1]->h0t[ic2];
75 ader_ref(1, 1) += cnt * th0t.X * th0t.X;
76 ader_ref(2, 1) += cnt * th0t.Y * th0t.X;
77 ader_ref(3, 1) += cnt * th0t.Z * th0t.X;
78 ader_ref(1, 2) += cnt * th0t.X * th0t.Y;
79 ader_ref(2, 2) += cnt * th0t.Y * th0t.Y;
80 ader_ref(3, 2) += cnt * th0t.Z * th0t.Y;
81 ader_ref(1, 3) += cnt * th0t.X * th0t.Z;
82 ader_ref(2, 3) += cnt * th0t.Y * th0t.Z;
83 ader_ref(3, 3) += cnt * th0t.Z * th0t.Z;
84 for (it = 1; it <= NTRK; ++it) {
85 tf0t = vk->ConstraintList[ic1]->f0t[it-1][ic2];
86 ader_ref(1, it * 3 + 1) += cnt * th0t.X * tf0t.X;
87 ader_ref(2, it * 3 + 1) += cnt * th0t.Y * tf0t.X;
88 ader_ref(3, it * 3 + 1) += cnt * th0t.Z * tf0t.X;
89 ader_ref(1, it * 3 + 2) += cnt * th0t.X * tf0t.Y;
90 ader_ref(2, it * 3 + 2) += cnt * th0t.Y * tf0t.Y;
91 ader_ref(3, it * 3 + 2) += cnt * th0t.Z * tf0t.Y;
92 ader_ref(1, it * 3 + 3) += cnt * th0t.X * tf0t.Z;
93 ader_ref(2, it * 3 + 3) += cnt * th0t.Y * tf0t.Z;
94 ader_ref(3, it * 3 + 3) += cnt * th0t.Z * tf0t.Z;
95 }
96 }
97 }
98
99
100 for(ic1=0; ic1<(int)vk->ConstraintList.size();ic1++){
101 for(ic2=0; ic2<vk->ConstraintList[ic1]->NCDim; ic2++){
102 for (it = 1; it <= NTRK; ++it) {
103 for (jt = it; jt <= NTRK; ++jt) {
104 Vect3DF tf0ti = vk->ConstraintList[ic1]->f0t[it-1][ic2];
105 Vect3DF tf0tj = vk->ConstraintList[ic1]->f0t[jt-1][ic2];
106 ader_ref(it*3 + 1, jt*3 + 1) += cnt * tf0ti.X * tf0tj.X;
107 ader_ref(it*3 + 2, jt*3 + 1) += cnt * tf0ti.Y * tf0tj.X;
108 ader_ref(it*3 + 3, jt*3 + 1) += cnt * tf0ti.Z * tf0tj.X;
109 ader_ref(it*3 + 1, jt*3 + 2) += cnt * tf0ti.X * tf0tj.Y;
110 ader_ref(it*3 + 2, jt*3 + 2) += cnt * tf0ti.Y * tf0tj.Y;
111 ader_ref(it*3 + 3, jt*3 + 2) += cnt * tf0ti.Z * tf0tj.Y;
112 ader_ref(it*3 + 1, jt*3 + 3) += cnt * tf0ti.X * tf0tj.Z;
113 ader_ref(it*3 + 2, jt*3 + 3) += cnt * tf0ti.Y * tf0tj.Z;
114 ader_ref(it*3 + 3, jt*3 + 3) += cnt * tf0ti.Z * tf0tj.Z;
115 }
116 }
117 }
118 }
119/* symmetrisation */
120 for (i=1; i<=NVar-1; ++i) {
121 for (j = i+1; j<=NVar; ++j) {
122 ader_ref(j,i) = ader_ref(i,j);
123 }
124 }
125//-------------------------------------------------------------------------
126/* several checks for debugging */
127//std::cout.precision(12);
128// for(ic1=0; ic1<(int)vk->ConstraintList.size();ic1++){
129// for(ic2=0; ic2<vk->ConstraintList[ic1]->NCDim; ic2++){
130// th0t = vk->ConstraintList[ic1]->h0t[ic2];
131//std::cout<<"h0t="<<th0t.X<<", "<<th0t.Y<<", "<<th0t.Z<<'\n';
132// for (it = 1; it <= NTRK; ++it) {
133// tf0t = vk->ConstraintList[ic1]->f0t[it-1][ic2];
134//std::cout<<"f0t="<<tf0t.X<<", "<<tf0t.Y<<", "<<tf0t.Z<<'\n';
135// } } }
136//if(NTRK==2){
137// for(i=1; i<=NVar; i++){std::cout<<" newmtx=";
138// for(j=1; j<=NVar; j++)std::cout<<ader_ref(j,i)<<", "; std::cout<<'\n';}
139//}
140//-------------------------------------------------------------------------
141// Weight matrix ready. Invert. Beware - DSINV destroys initial matrix!
142 noinit_vector<double*> ta (NVar+1);
143 noinit_vector<double> tab ((NVar+1)*(NVar+1));
144 for(i=0; i<NVar+1; i++){ ta[i] = tab.data() + i*(NVar+1);}
145 for (i=1; i<=NVar; ++i) for (j = i; j<=NVar; ++j) ta[i][j] = ta[j][i] = ader_ref(i,j); //Make copy for failure treatment
146 dsinv(NVar, ader, vkalNTrkM*3+3, &IERR);
147 if ( IERR != 0) {
148 noinit_vector<double*> tv (NVar+1);
149 noinit_vector<double> tvb ((NVar+1)*(NVar+1));
150 noinit_vector<double*> tr (NVar+1);
151 noinit_vector<double> trb ((NVar+1)*(NVar+1));
152 noinit_vector<double> tw (NVar+1);
153 for(i=0; i<NVar+1; i++){ tv[i] = tvb.data() + i*(NVar+1); tr[i] = trb.data() + i*(NVar+1);}
154
155 vkSVDCmp( ta.data(), NVar, NVar, tw.data(), tv.data());
156
157 double tmax=0;
158 for(i=1; i<NVar+1; i++) if(fabs(tw[i])>tmax)tmax=fabs(tw[i]);
159 for(i=1; i<NVar+1; i++) if(fabs(tw[i])/tmax < 1.e-18) tw[i]=0.;
160 for(i=1; i<=NVar; i++){ for(j=1; j<=NVar; j++){
161 tr[i][j]=0.; for(int k=1; k<=NVar; k++) if(tw[k]!=0.) tr[i][j] += ta[i][k]*tv[j][k]/tw[k];
162 }}
163
164 for (i=1; i<=NVar; ++i) for (j=1; j<=NVar; ++j) ader_ref(i,j)=tr[i][j];
165
166 IERR=0; //return IERR;
167 }
168 //------ Check matrix inversion quality
169 double maxDiff=0.;
170 for( i=1; i<=NVar; i++){ for(j=i; j<=NVar; j++){
171 double mcheck=0.; for(int k=1; k<=NVar; k++) mcheck+=ta[i][k]*ader_ref(k,j);
172 if(i!=j) maxDiff = (maxDiff > std::abs(mcheck)) ? maxDiff : std::abs(mcheck);
173 if(i==j) maxDiff = (maxDiff > std::abs(1.-mcheck)) ? maxDiff : std::abs(1.-mcheck);
174 } }
175 //---------------------------------------------------------------------------------------
176 if(maxDiff>0.1)return -1;
177/* ---------------------------------------- */
178 } else {
179/* ---------------------------------------- */
180/* Simple and fast without constraints */
181 for (i=1; i<=NVar; i++) {
182 for (j=1; j<=NVar; j++) {
183 ader_ref(i,j)=0.;
184 }
185 }
186 double vcov[6]={vk->fitVcov[0],vk->fitVcov[1],vk->fitVcov[2],vk->fitVcov[3],vk->fitVcov[4],vk->fitVcov[5]};
187 ader_ref(1,1) = vcov[0];
188 ader_ref(1,2) = vcov[1];
189 ader_ref(2,2) = vcov[2];
190 ader_ref(1,3) = vcov[3];
191 ader_ref(2,3) = vcov[4];
192 ader_ref(3,3) = vcov[5];
193 ader_ref(2,1) = ader_ref(1,2);
194 ader_ref(3,1) = ader_ref(1,3);
195 ader_ref(3,2) = ader_ref(2,3);
196
197 for (it=1; it<=NTRK; it++) {
198 t_trk=vk->tmpArr[it-1].get();
199 ader_ref(1, it*3 + 1) = -vcov[0] * t_trk->wbci[0]
200 - vcov[1] * t_trk->wbci[1]
201 - vcov[3] * t_trk->wbci[2];
202 ader_ref(2, it*3 + 1) = -vcov[1] * t_trk->wbci[0]
203 - vcov[2] * t_trk->wbci[1]
204 - vcov[4] * t_trk->wbci[2];
205 ader_ref(3, it*3 + 1) = -vcov[3] * t_trk->wbci[0]
206 - vcov[4] * t_trk->wbci[1]
207 - vcov[5] * t_trk->wbci[2];
208 ader_ref(1, it*3 + 2) = -vcov[0] * t_trk->wbci[3]
209 - vcov[1] * t_trk->wbci[4]
210 - vcov[3] * t_trk->wbci[5];
211 ader_ref(2, it*3 + 2) = -vcov[1] * t_trk->wbci[3]
212 - vcov[2] * t_trk->wbci[4]
213 - vcov[4] * t_trk->wbci[5];
214 ader_ref(3, it*3 + 2) = -vcov[3] * t_trk->wbci[3]
215 - vcov[4] * t_trk->wbci[4]
216 - vcov[5] * t_trk->wbci[5];
217 ader_ref(1, it*3 + 3) = -vcov[0] * t_trk->wbci[6]
218 - vcov[1] * t_trk->wbci[7]
219 - vcov[3] * t_trk->wbci[8];
220 ader_ref(2, it*3 + 3) = -vcov[1] * t_trk->wbci[6]
221 - vcov[2] * t_trk->wbci[7]
222 - vcov[4] * t_trk->wbci[8];
223 ader_ref(3, it*3 + 3) = -vcov[3] * t_trk->wbci[6]
224 - vcov[4] * t_trk->wbci[7]
225 - vcov[5] * t_trk->wbci[8];
226 ader_ref(it*3 + 1, 1) = ader_ref(1, it*3 + 1);
227 ader_ref(it*3 + 1, 2) = ader_ref(2, it*3 + 1);
228 ader_ref(it*3 + 1, 3) = ader_ref(3, it*3 + 1);
229 ader_ref(it*3 + 2, 1) = ader_ref(1, it*3 + 2);
230 ader_ref(it*3 + 2, 2) = ader_ref(2, it*3 + 2);
231 ader_ref(it*3 + 2, 3) = ader_ref(3, it*3 + 2);
232 ader_ref(it*3 + 3, 1) = ader_ref(1, it*3 + 3);
233 ader_ref(it*3 + 3, 2) = ader_ref(2, it*3 + 3);
234 ader_ref(it*3 + 3, 3) = ader_ref(3, it*3 + 3);
235 }
236
237
238 for (it = 1; it<=NTRK; ++it) {
239 t_trk=vk->tmpArr[it-1].get();
240 for (jt=1; jt<=NTRK; ++jt) {
241 int j3 = jt*3;
242 int i3 = it*3;
243 ader_ref( i3+1, j3+1) = -t_trk->wbci[0]*ader_ref(1, j3+1) - t_trk->wbci[1]*ader_ref(2, j3+1) - t_trk->wbci[2]*ader_ref(3, j3+1);
244 ader_ref( i3+2, j3+1) = -t_trk->wbci[3]*ader_ref(1, j3+1) - t_trk->wbci[4]*ader_ref(2, j3+1) - t_trk->wbci[5]*ader_ref(3, j3+1);
245 ader_ref( i3+3, j3+1) = -t_trk->wbci[6]*ader_ref(1, j3+1) - t_trk->wbci[7]*ader_ref(2, j3+1) - t_trk->wbci[8]*ader_ref(3, j3+1);
246 ader_ref( i3+1, j3+2) = -t_trk->wbci[0]*ader_ref(1, j3+2) - t_trk->wbci[1]*ader_ref(2, j3+2) - t_trk->wbci[2]*ader_ref(3, j3+2);
247 ader_ref( i3+2, j3+2) = -t_trk->wbci[3]*ader_ref(1, j3+2) - t_trk->wbci[4]*ader_ref(2, j3+2) - t_trk->wbci[5]*ader_ref(3, j3+2);
248 ader_ref( i3+3, j3+2) = -t_trk->wbci[6]*ader_ref(1, j3+2) - t_trk->wbci[7]*ader_ref(2, j3+2) - t_trk->wbci[8]*ader_ref(3, j3+2);
249 ader_ref( i3+1, j3+3) = -t_trk->wbci[0]*ader_ref(1, j3+3) - t_trk->wbci[1]*ader_ref(2, j3+3) - t_trk->wbci[2]*ader_ref(3, j3+3);
250 ader_ref( i3+2, j3+3) = -t_trk->wbci[3]*ader_ref(1, j3+3) - t_trk->wbci[4]*ader_ref(2, j3+3) - t_trk->wbci[5]*ader_ref(3, j3+3);
251 ader_ref( i3+3, j3+3) = -t_trk->wbci[6]*ader_ref(1, j3+3) - t_trk->wbci[7]*ader_ref(2, j3+3) - t_trk->wbci[8]*ader_ref(3, j3+3);
252 if (it == jt) {
253 ader_ref( i3+1, i3+1) += t_trk->wci[0];
254 ader_ref( i3+1, i3+2) += t_trk->wci[1];
255 ader_ref( i3+2, i3+1) += t_trk->wci[1];
256 ader_ref( i3+2, i3+2) += t_trk->wci[2];
257 ader_ref( i3+1, i3+3) += t_trk->wci[3];
258 ader_ref( i3+3, i3+1) += t_trk->wci[3];
259 ader_ref( i3+2, i3+3) += t_trk->wci[4];
260 ader_ref( i3+3, i3+2) += t_trk->wci[4];
261 ader_ref( i3+3, i3+3) += t_trk->wci[5];
262 }
263 }
264 }
265//for(int ii=1; ii<=9; ii++)std::cout<<ader_ref(ii,ii)<<", "; std::cout<<__func__<<" fast full m NEW"<<'\n';
266 if( !vk->ConstraintList.empty() && !useWeightScheme ){
267//---------------------------------------------------------------------
268// Covariance matrix with constraints a la Avery.
269// ader_ref() should contain nonconstraint covariance matrix
270//---------------------------------------------------------------------
271 long int totNC=0; //total number of constraints
272 std::vector<std::vector< Vect3DF> > tf0t; // derivative collectors
273 std::vector< Vect3DF > th0t; // derivative collectors
274 std::vector< double > taa; // derivative collectors
275 std::vector< Vect3DF > tmpVec;
276 for(int ii=0; ii<(int)vk->ConstraintList.size();ii++){
277 totNC += vk->ConstraintList[ii]->NCDim;
278 for(ic=0; ic<(int)vk->ConstraintList[ii]->NCDim; ic++){
279 taa.push_back( vk->ConstraintList[ii]->aa[ic] );
280 th0t.push_back( vk->ConstraintList[ii]->h0t[ic] );
281 tmpVec.clear();
282 for(it=0; it<(int)vk->ConstraintList[ii]->f0t.size(); it++){
283 tmpVec.push_back( vk->ConstraintList[ii]->f0t[it][ic] );
284 }
285 tf0t.push_back( tmpVec );
286 }
287 }
288// R,RC[ic][i]
289 double **R =new double*[totNC]; for(ic=0; ic<totNC; ic++) R[ic]=new double[NVar];
290 double **RC=new double*[totNC]; for(ic=0; ic<totNC; ic++)RC[ic]=new double[NVar];
291 double *RCRt=new double[totNC*totNC];
292 for(ic=0; ic<totNC; ic++){
293 R[ic][0]=th0t[ic].X;
294 R[ic][1]=th0t[ic].Y;
295 R[ic][2]=th0t[ic].Z;
296 for(it=1; it<=NTRK; it++){
297 R[ic][it*3+0]=tf0t[ic][it-1].X;
298 R[ic][it*3+1]=tf0t[ic][it-1].Y;
299 R[ic][it*3+2]=tf0t[ic][it-1].Z;
300 }
301 }
302// R*Cov matrix
303 for(ic=0; ic<totNC; ic++){
304 for(j=0; j<NVar; j++){ RC[ic][j]=0;
305 for(i=0; i<NVar; i++) RC[ic][j] += R[ic][i]*ader_ref(i+1,j+1);
306 }
307 }
308// R*Cov*Rt matrix - Lagrange multiplyers errors
309 for(ic=0; ic<totNC; ic++){
310 for(jc=0; jc<totNC; jc++){ RCRt[ic*totNC + jc] =0.;
311 for(i=0; i<NVar; i++) RCRt[ic*totNC + jc] += RC[ic][i]*R[jc][i];
312 }
313 }
314 dsinv(totNC, RCRt, totNC, &IERR);
315 if ( IERR != 0) return IERR;
316// Correction matrix
317 for(i=0; i<NVar; i++){
318 for(j=0; j<NVar; j++){ double COR=0.;
319 for(ic=0; ic<totNC; ic++){
320 for(jc=0; jc<totNC; jc++){
321 COR += RC[ic][i]*RC[jc][j]*RCRt[ic*totNC +jc];
322 }
323 }
324 ader_ref(i+1, j+1) -= COR;
325 }
326 }
327// Delete temporary matrices
328 for(ic=0; ic<totNC; ic++) delete[] R[ic];
329 delete[] R;
330 for(ic=0; ic<totNC; ic++) delete[] RC[ic];
331 delete[] RC;
332 delete[] RCRt;
333//for(int ii=1; ii<=9; ii++)std::cout<<ader_ref(ii,ii)<<", "; std::cout<<__func__<<" avery full m NEW"<<'\n';
334 } //end of Avery matrix
335
336
337
338 } // End of global IF() for matrix type selection
339
340//if(NTRK==2){
341// for(i=1; i<=NVar; i++){std::cout<<__func__" new covfull=";
342// for(j=1; j<=NVar; j++)std::cout<<ader_ref(j,i)<<", "; std::cout<<'\n';}
343//}
344
345/* --Conversion to (X,Y,Z,Px,Py,Pz) form */
346 for (i = 1; i <= 6; ++i) {
347 for (j = 1; j <= 6; ++j) {
348 verr[i-1][j-1] = 0.;
349 for (ic=1; ic<=NVar; ++ic) {
350 if(dcv_ref(i, ic)==0.) continue;
351 for (jc=1; jc<=NVar; ++jc) {
352 if(dcv_ref(j, jc)==0.) continue;
353 verr[i-1][j-1] += dcv_ref(i, ic) * ader_ref(ic, jc) * dcv_ref(j, jc);
354 }
355 }
356 }
357 }
358//for(int ii=1; ii<=6; ii++)std::cout<<verr[ii-1][ii-1]<<", "; std::cout<<" final m NEW"<<'\n';
359 vk->existFullCov = 1;
360 return 0;
361}
362#undef dcv_ref
363#undef ader_ref
364
365#undef useWeightScheme
366
367} /* End of VKalVrtCore namespace*/
368
#define vkalNTrkM
Definition CommonPars.h:22
#define ader_ref(a_1, a_2)
Definition FullMtx.cxx:17
#define useWeightScheme
Definition VtCFitE.cxx:23
#define dcv_ref(a_1, a_2)
std::vector< std::unique_ptr< VKTrack > > TrackList
std::vector< std::unique_ptr< VKConstraintBase > > ConstraintList
std::vector< std::unique_ptr< TWRK > > tmpArr
Ensure that the ATLAS eigen extensions are properly loaded.
std::vector< T, boost::noinit_adaptor< std::allocator< T > > > noinit_vector
A variant on std::vector which leaves its contents uninitialized by default.
void vkSVDCmp(double **a, int m, int n, double w[], double **v)
void FullMTXfill(VKVertex *vk, double *ader)
Definition FullMtx.cxx:19
void dsinv(long int n, double *a, long int DIM, long int *ifail) noexcept
int getFullVrtCov(VKVertex *vk, double *ader, const double *dcv, double verr[6][6])
Definition VtCFitE.cxx:25