82 {
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99 bool permitFlips = _allowFlips;
100 bool lPickHits = _allowDrops;
101
102
103 int i;
105 int lPicked = 0;
106
107 register double chisqold;
108 double chisqnew, chichange;
109 double chitest = 0.01;
111 int nActive = 0;
112
113
114
115
116
117
118
119
120
121 setLastChisq( -1. );
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143 int nhits = hitlist.nHit();
144 std::vector<double> delChi( nhits, 0 );
145 std::vector<std::vector<double>> deriv( nhits );
146
147 TrkParams& params = *( theTraj.
parameters() );
148
149 int npar = theTraj.
nPar();
150
151
152
153
154 bool l3d = ( npar > 3 );
155 const int minZ = l3d ? 2 : 0;
156 const int minXY = npar - minZ;
157 const int minAct = minZ + minXY;
158
159 HepSymMatrix vparam( npar, 0 );
160 HepVector diffsum( npar );
161 HepVector delpar( npar );
162
163 std::vector<std::vector<double>>::iterator ideriv = deriv.begin();
164 std::vector<double>::iterator idelChi = delChi.begin();
165 assert( ( (int)deriv.size() ) == ( hitlist.end() - hitlist.begin() ) );
167 ++ihit, ++ideriv, ++idelChi )
168 {
169 ideriv->resize( npar );
170 if ( ihit->isActive() )
171 {
172 nActive++;
176 {
178 nXY++;
179 }
180 }
181
182
183
184
185
186 }
187 if ( nXY < minXY ||
nZ < minZ || nActive < minAct )
188 {
189 status.setFailure( 11, "Not enough hits in TrkHelixFitter! " );
190 return status;
191 }
192
193
194
195
196
197
198
199
200
201
202
203
204
205 HepVector derivs( npar );
206
207 TrkErrCode calcResult;
208
209 size_t itermax = 12;
211 {
212 bool mustIterate( false );
213 chisqold = 0.0;
214 for ( i = 0; i < npar; i++ ) diffsum[i] = 0.0;
215 vparam *= 0.0;
216
217
218 std::vector<std::vector<double>>::iterator ideriv = deriv.begin();
219 std::vector<double>::iterator idelChi = delChi.begin();
220 assert( ( (int)deriv.size() ) == ( hitlist.end() - hitlist.begin() ) );
222 ++ihit, ++ideriv, ++idelChi )
223 {
224
225
227 double deltaChiNew;
229 {
231 {
232 calcResult = ihit->getFitStuff( derivs, deltaChiNew );
233 for ( i = 0; i < npar; ++i ) ( *ideriv )[i] = derivs[i];
234
235
236
237
238
239
240 }
241 else { calcResult = ihit->getFitStuff( deltaChiNew ); }
242 }
244 {
246 {
247 cout << "ErrMsg(warning) TrkHelixFitter:"
248 << "unable to getFitStuff for hit " << *ihit << endl;
249 }
250 ihit->setUsability( false );
251 continue;
252 }
253 mustIterate = ( mustIterate || ( calcResult.
success() != 1 ) );
254 *idelChi = deltaChiNew;
256 {
257 cout << ( ihit - hitlist.begin() );
258 ihit->print( std::cout );
259 cout << " dChi " << *idelChi << " amb " << ihit->ambig() << " resid " << ihit->resid()
260 << " rms " << ihit->hitRms() << " hitlen " << ihit->hitLen() << " fltlen "
261 << ihit->fltLen() << endl;
262 }
263 if ( ihit->isActive() == false )
264 {
265 if (
m_debug ) std::cout <<
"SKIP not active hit" << std::endl;
266 continue;
267 }
268 chisqold += deltaChiNew * deltaChiNew;
269
270 for ( i = 0; i < npar; ++i )
271 {
272 diffsum[i] += ( *ideriv )[i] * deltaChiNew;
273 for ( int j = 0; j < i + 1; ++j )
274 { vparam.fast( i + 1, j + 1 ) += ( *ideriv )[i] * ( *ideriv )[j]; }
275 }
276 }
277
278
279 int ierr;
280 vparam.invert( ierr );
281 if ( ierr )
282 {
284 {
285 cout << "ErrMsg(warning) TrkHelixFitter:"
286 << "Matrix inversion failed " << endl;
287 }
288 status.setFailure( 12, "Matrix inversion failed in TrkHelixFitter" );
289
290 }
291 delpar = vparam * ( -diffsum );
292 if (
m_debug ) { cout <<
" delpar = " << delpar << endl; }
293
294
295 if ( fabs( delpar[1] ) > 1. )
296 {
298 {
299 cout << "ErrMsg(warning) TrkHelixFitter:"
300 << "Pathological fit " << endl;
301 }
302 status.setFailure( 13, "Pathological fit in TrkHelixFitter." );
303
304 }
305
306 for ( i = 0; i < npar; ++i ) params.
parameter()[i] += delpar[i];
308
309
310
311
312
313 chisqnew = 0.0;
314 lPicked = 0;
315 double bigDelChi = 0.0;
317
318 mustIterate =
319 ( mustIterate || (
iter <= 2 && lPickHits ) );
320 ideriv = deriv.begin();
321 idelChi = delChi.begin();
323 ++ihit, ++ideriv, ++idelChi )
324 {
325 if (
m_debug ) { ihit->print( std::cout ); }
326 if ( !ihit->isUsable() )
327 {
328 if (
m_debug ) { std::cout <<
"hit NOT usable " << std::endl; }
329 continue;
330 }
331
332 for ( i = 0; i < npar; i++ ) { *idelChi += ( *ideriv )[i] * delpar[i]; }
333 if ( ihit->isActive() ) chisqnew += *idelChi * *idelChi;
334
335
336 if ( !mustIterate && lPickHits )
337 {
338 double abDelChi = fabs( *idelChi );
339
340 if ( abDelChi <=
nSigmaCut[ihit->layerNumber()] )
341 {
343 {
344 std::cout <<
"abDelChi " << abDelChi <<
"<?" <<
nSigmaCut[ihit->layerNumber()]
345 << std::endl;
346 }
347 if ( ihit->isActive() == 0 )
348 {
349 ihit->setActivity( 1 );
350 if (
m_debug ) { cout <<
"set ACTIVE, Added " << endl; }
351 lPicked = 1;
352 nActive++;
356 {
358 nXY++;
359 }
360 }
361 }
362 else
363 {
364 if ( ihit->isActive() )
365 {
366 if ( abDelChi > bigDelChi )
367 {
369 {
370 std::cout << "bigest set INACTIVE, abDelChi = " << abDelChi << ">"
371 <<
nSigmaCut[ihit->layerNumber()] <<
" bigDelChi=" << bigDelChi
372 << std::endl;
373 }
374 bigDelChi = abDelChi;
375 bigHit = ihit;
376 }
377 }
378 }
379 }
380 }
381
382
383 if ( lPickHits )
384 {
385 int lDrop = 0;
386 if ( bigHit != hitlist.end() && ( nActive > minAct ) )
387 {
389 {
390 nXY--;
391 lDrop = 1;
392 }
394 {
396 lDrop = 1;
397 }
399 {
401 nXY--;
402 lDrop = 1;
403 }
404 if ( lDrop == 1 )
405 {
406 lPicked = 1;
407 nActive--;
410 {
411 std::cout << "---deactivate hit!! delChi2=" << bigDelChi << std::endl;
412 std::cout << "---";
413 bigHit->
print( std::cout );
414 std::cout << "--------------------!! " << std::endl;
415 }
416 }
417 }
418 }
419
420
421 chichange = chisqold - chisqnew;
422 if (
m_debug ) { cout <<
"chisq from " << chisqold <<
" -> " << chisqnew << endl; }
423 if ( chichange < -0.5 && !mustIterate && lPicked == 0 )
424 {
426 {
427 cout << "ErrMsg(warning)"
428 << " blowing up: " << chichange << endl;
429 }
430
431 setLastChisq( chisqnew );
432 status.setFailure( 1 );
433
434 if (
m_debug ) std::cout <<
"failure 1 " << std::endl;
435 break;
436 }
437 else if ( chichange < chitest && !mustIterate && lPicked == 0 )
438 {
439
440 status.setSuccess( 1 );
441 setLastChisq( chisqnew );
442 if (
m_debug ) std::cout <<
"success 1 " << std::endl;
443 break;
444 }
445
446 if (
iter == itermax )
447 {
448 setLastChisq( chisqnew );
449 status.setSuccess( 2 );
450 if (
m_debug ) std::cout <<
"success 2 " << std::endl;
451 }
452 }
453
454
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491 return status;
492}
static double nSigmaCut[43]
TrkErrCode updateMeasurement(TrkHitOnTrk &hot, const TrkDifTraj *traj=0, bool maintainAmbiguity=false) const
void setActivity(bool turnOn)
virtual TrkEnums::TrkViewInfo whatView() const =0
virtual void print(std::ostream &) const
TrkHitOnTrkIter< TrkHotList::iterator_traits > nc_hot_iterator
HepSymMatrix & covariance()