91 {
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108 bool permitFlips = _allowFlips;
109 bool lPickHits = _allowDrops;
110
111
112 int i;
114 int lPicked = 0;
115
116 register double chisqold;
117 double chisqnew, chichange;
118 double chitest = 0.01;
120 int nActive = 0;
121
122
123
124
125
126
127
128
129
130 setLastChisq(-1.);
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151 int ncluster(0);
154 }
155
156 int nhits = hitlist.nHit();
157 std::vector<double> delChi(nhits,0);
158 std::vector<double> XdelChi(ncluster,0);
159 std::vector<double> VdelChi(ncluster,0);
160 std::vector<std::vector<double> > deriv(nhits);
161 std::vector<std::vector<double> > Xderiv(ncluster);
162 std::vector<std::vector<double> > Vderiv(ncluster);
163
165
166 int npar = theTraj.
nPar();
167
168
169
170
171 bool l3d = (npar > 3);
172 const int minZ = l3d ? 2 : 0;
173 const int minXY = npar - minZ;
174 const int minAct = minZ + minXY;
175
176 HepSymMatrix vparam(npar,0);
177 HepVector diffsum(npar);
178 HepVector delpar(npar);
179
180 std::vector<std::vector<double> >::iterator ideriv = deriv.begin();
181 std::vector<std::vector<double> >::iterator iXderiv = Xderiv.begin();
182 std::vector<std::vector<double> >::iterator iVderiv = Vderiv.begin();
183 std::vector<double>::iterator idelChi = delChi.begin();
184 std::vector<double>::iterator iXdelChi = XdelChi.begin();
185 std::vector<double>::iterator iVdelChi = VdelChi.begin();
186 assert(((int)deriv.size()) ==(hitlist.end()-hitlist.begin()));
188 ideriv->resize(npar);
189 if (ihit->isActive()) {
190 nActive++;
195 nXY++;
196 }
197 }
198
200 iXderiv->resize(npar);
201 iVderiv->resize(npar);
202 iXderiv++;
203 iVderiv++;
204 }
205
206
207
208
209 }
210 if (nXY < minXY ||
nZ < minZ || nActive < minAct) {
211 status.setFailure(11,"Not enough hits in TrkHelixFitter! ");
212 return status;
213 }
214
215
216
217
218
219
220
221
222
223
224
225
226
227 HepVector derivs(npar);
228 HepVector Xderivs(npar);
229 HepVector Vderivs(npar);
230
232
233 size_t itermax = 12;
236 bool mustIterate(false);
237 chisqold = 0.0;
238 for (i = 0; i < npar; i++) diffsum[i] = 0.0;
239 vparam *= 0.0;
240
241
242 std::vector<std::vector<double> >::iterator ideriv = deriv.begin();
243 std::vector<std::vector<double> >::iterator iXderiv = Xderiv.begin();
244 std::vector<std::vector<double> >::iterator iVderiv = Vderiv.begin();
245 std::vector<double>::iterator idelChi = delChi.begin();
246 std::vector<double>::iterator iXdelChi = XdelChi.begin();
247 std::vector<double>::iterator iVdelChi = VdelChi.begin();
248 assert(((int)deriv.size())==(hitlist.end()-hitlist.begin()));
250
251
253 double deltaChiNew;
254 double XdeltaChiNew;
255 double VdeltaChiNew;
260 calcResult = cgemHot->
getFitStuff(Xderivs, XdeltaChiNew, Vderivs, VdeltaChiNew);
261 for (i=0; i<npar; ++i){
262 (*iXderiv)[i] = Xderivs[i];
263 (*iVderiv)[i] = Vderivs[i];
264 }
265 }else{
266 calcResult = ihit->getFitStuff(derivs, deltaChiNew);
267 for (i=0; i<npar; ++i) (*ideriv)[i] = derivs[i];
268 }
269
270
271
272
273
274
275
276 } else {
279 calcResult = cgemHot->
getFitStuff(XdeltaChiNew, VdeltaChiNew);
280 }else{
281 calcResult = ihit->getFitStuff(deltaChiNew);
282 }
283 }
284 }
287 cout<<"ErrMsg(warning) TrkHelixFitter:"
288 << "unable to getFitStuff for hit " << *ihit << endl;
289 }
290 ihit->setUsability(false);
292 iXderiv++;
293 iVderiv++;
294 iXdelChi++;
295 iVdelChi++;
296 }
297 continue;
298 }
299 mustIterate = (mustIterate || (calcResult.
success() != 1));
301 *iXdelChi = XdeltaChiNew;
302 *iVdelChi = VdeltaChiNew;
303 }else{
304 *idelChi = deltaChiNew;
305 }
306
308 cout << (ihit-hitlist.begin());
309 ihit->
print(std::cout);
310 cout << " dChi " << *idelChi
311 << " amb " << ihit->ambig()
312 << " resid " << ihit->resid()
313 << " rms " << ihit->hitRms()
314 << " hitlen " << ihit->hitLen()
315 << " fltlen " << ihit->fltLen() << endl;
316 }
317
318 if (ihit->isActive() == false) {
319 if(
m_debug) std::cout<<
"SKIP not active hit"<< std::endl;
321 iXderiv++;
322 iVderiv++;
323 iXdelChi++;
324 iVdelChi++;
325 }
326 continue;
327 }
329 chisqold += XdeltaChiNew * XdeltaChiNew;
330 chisqold += VdeltaChiNew * VdeltaChiNew;
331 for (i = 0; i < npar; ++i) {
332 diffsum[i] += (*iXderiv)[i] * XdeltaChiNew;
333 diffsum[i] += (*iVderiv)[i] * VdeltaChiNew;
334 for (int j = 0; j < i+1; ++j) {
335 vparam.fast(i+1,j+1) += (*iXderiv)[i] * (*iXderiv)[j];
336 vparam.fast(i+1,j+1) += (*iVderiv)[i] * (*iVderiv)[j];
337 }
338 }
339 }else{
340 chisqold += deltaChiNew * deltaChiNew;
341 for (i = 0; i < npar; ++i) {
342 diffsum[i] += (*ideriv)[i] * deltaChiNew;
343 for (int j = 0; j < i+1; ++j) {
344 vparam.fast(i+1,j+1) += (*ideriv)[i] * (*ideriv)[j];
345 }
346 }
347 }
349 iXderiv++;
350 iVderiv++;
351 iXdelChi++;
352 iVdelChi++;
353 }
356 cout<<ihit->layerNumber()<<" "<<setw(12)<<XdeltaChiNew<<" || "<<setw(12)<<(Xderivs)[0]<<" "<<setw(12)<<(Xderivs)[1]<<" "<<setw(12)<<(Xderivs)[2]<<" "<<setw(12)<<(Xderivs)[3]<<" "<<setw(12)<<(Xderivs)[4]<<endl;
357 cout<<ihit->layerNumber()<<" "<<setw(12)<<VdeltaChiNew<<" || "<<setw(12)<<(Vderivs)[0]<<" "<<setw(12)<<(Vderivs)[1]<<" "<<setw(12)<<(Vderivs)[2]<<" "<<setw(12)<<(Vderivs)[3]<<" "<<setw(12)<<(Vderivs)[4]<<endl;
358 }else cout<<ihit->layerNumber()<<" "<<setw(12)<<deltaChiNew<<" || "<<setw(12)<<(*ideriv)[0]<<" "<<setw(12)<<(*ideriv)[1]<<" "<<setw(12)<<(*ideriv)[2]<<" "<<setw(12)<<(*ideriv)[3]<<" "<<setw(12)<<(*ideriv)[4]<<endl;
359 }
360 }
361
362
363
364 int ierr;
365 vparam.invert(ierr);
366 if (ierr) {
368 cout<<"ErrMsg(warning) TrkHelixFitter:"
369 << "Matrix inversion failed " << endl;
370 }
371 status.setFailure(12, "Matrix inversion failed in TrkHelixFitter");
372
373 }
374 delpar = vparam * (-diffsum);
376 cout << " delpar = "<<delpar << endl;
377 }
378
379
380 if (fabs(delpar[1]) > 1.) {
382 cout<<"ErrMsg(warning) TrkHelixFitter:"
383 << "Pathological fit " << endl;
384 }
385 status.setFailure(13, "Pathological fit in TrkHelixFitter.");
386
387 }
388
389 for (i = 0; i < npar; ++i) params.
parameter()[i] += delpar[i];
391 cout <<
" params "<<params.
parameter() << endl;
392 }
393
394
395
396
397
398 chisqnew = 0.0;
399 lPicked = 0;
400 double bigDelChi = 0.0;
402
403 mustIterate = (mustIterate || (
iter <= 2 && lPickHits));
404 ideriv = deriv.begin();
405 iXderiv = Xderiv.begin();
406 iVderiv = Vderiv.begin();
407 idelChi = delChi.begin();
408 iXdelChi = XdelChi.begin();
409 iVdelChi = VdelChi.begin();
412 ihit->print(std::cout);
413 }
414 if(!ihit->isUsable()){
415 if(
m_debug) { std::cout<<
"hit NOT usable "<< std::endl; }
417 iXderiv++;
418 iVderiv++;
419 iXdelChi++;
420 iVdelChi++;
421 }
422 continue;
423 }
424
426 for (i = 0; i < npar; i++) {
427 *iXdelChi += (*iXderiv)[i] * delpar[i];
428 *iVdelChi += (*iVderiv)[i] * delpar[i];
429 }
430 if (ihit->isActive()){
431 chisqnew += *iXdelChi * *iXdelChi;
432 chisqnew += *iVdelChi * *iVdelChi;
433 }
434 }else{
435 for (i = 0; i < npar; i++) {
436 *idelChi += (*ideriv)[i] * delpar[i];
437 }
438 if (ihit->isActive()) chisqnew += *idelChi * *idelChi;
439 }
440
441 if (!mustIterate && lPickHits) {
442 double abDelChi;
443 if(
typeid(*ihit)==
typeid(
CgemHitOnTrack)) abDelChi = fabs(*iXdelChi + *iVdelChi)/2;
444 else abDelChi = fabs(*idelChi);
445
446 if (abDelChi <=
nSigmaCut[ihit->layerNumber()] ) {
448 std::cout<< "abDelChi "<<abDelChi
449 <<
"<?"<<
nSigmaCut[ihit->layerNumber()] << std::endl;
450 }
451 if (ihit->isActive() == 0) {
452 ihit->setActivity(1);
453 if(
m_debug){ cout <<
"set ACTIVE, Added " << endl; }
454 lPicked = 1;
455 nActive++;
460 nXY++;
461 }
462 }
463 } else {
464 if (ihit->isActive()) {
465 if (abDelChi > bigDelChi) {
467 std::cout<<"bigest set INACTIVE, abDelChi = "<<abDelChi
468 <<
">"<<
nSigmaCut[ihit->layerNumber()] <<
" bigDelChi=" <<bigDelChi<< std::endl; }
469 bigDelChi = abDelChi;
470 bigHit = ihit;
471 }
472 }
473 }
474 }
476 iXderiv++;
477 iVderiv++;
478 iXdelChi++;
479 iVdelChi++;
480 }
481 }
482
483
484 if (lPickHits) {
485 int lDrop = 0;
486 if (bigHit != hitlist.end() && (nActive > minAct)) {
488 nXY--;
489 lDrop = 1;
492 lDrop = 1;
494 nXY > minXY) {
496 nXY--;
497 lDrop = 1;
498 }
499 if (lDrop == 1) {
500 lPicked = 1;
501 nActive--;
502 bigHit->setActivity(0);
504 std::cout<<"---deactivate hit!! delChi2="<<bigDelChi<< std::endl;
505 std::cout<<"---";
506 bigHit->print(std::cout);
507 std::cout<<"--------------------!! "<< std::endl;
508 }
509 }
510 }
511 }
512
513
514 chichange = chisqold - chisqnew;
516 cout << "chisq from "<<chisqold << " -> " << chisqnew << endl;
517 }
518 if (chichange < -0.5 && !mustIterate && lPicked == 0) {
520 cout<<"ErrMsg(warning)" << " blowing up: " << chichange << endl;
521 }
522
523 setLastChisq(chisqnew);
524 status.setFailure(1);
525
526 if(
m_debug) std::cout<<
"failure 1 "<< std::endl;
527 break;
528 } else if (chichange < chitest && !mustIterate && lPicked ==0){
529
530 status.setSuccess(1);
531 setLastChisq(chisqnew);
532 if(
m_debug) std::cout<<
"success 1 "<< std::endl;
533 break;
534 }
535
536 if (
iter == itermax) {
537 setLastChisq(chisqnew);
538 status.setSuccess(2);
539 if(
m_debug) std::cout<<
"success 2 "<< std::endl;
540 }
541 }
542
543
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580 return status;
581}
TrkErrCode getFitStuff(HepVector &derivs, HepVector &derivs2, double &sigma1, double &sigma2, double &deltaChi1, double &deltaChi2) const
void print(std::ostream &ostr) const
static double nSigmaCut[43]
TrkErrCode updateMeasurement(TrkHitOnTrk &hot, const TrkDifTraj *traj=0, bool maintainAmbiguity=false) const
HepSymMatrix & covariance()