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