419{
420
421
422 int i, j, k, ik, ij, ist, nderlc, ndergl, ndf;
423 int ja = -1;
424 int jb = 0;
425 int nagbn = 0;
426
427 double rmeas, wght, rms, cutval;
428
429 double summ = 0.0;
430 int nsum = 0;
431 nst = 0;
432 nst = arest.size();
433
434
435
436
437 if (itert < 2 && single_fit != 1)
438 {
439 if (
debug_mode) cout <<
"Store equation no: " <<
n << endl;
440
441 for (i=0; i<nst; i++)
442 {
443 storeind.push_back(indst[i]);
444 storeare.push_back(arest[i]);
445 storenl.push_back(arenl[i]);
446
447 if (arenl[i] != 0.) arest[i] = 0.0;
448 }
449
450 arenl.clear();
451
452 storeplace.push_back(storeind.size());
453
454 if (
verbose_mode) cout <<
"StorePlace size = " << storeplace[
n] << endl;
455 if (
verbose_mode) cout <<
"StoreInd size = " << storeind.size() << endl;
456 }
457
458
459 for (i=0; i<nalc; i++)
460 {
461 blvec[i] = 0.0;
462
463 for (j=0; j<nalc; j++) {clmat[i][j] = 0.0;}
464 }
465
466 for (i=0; i<nagb; i++) {indnz[i] = -1;}
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
492
493
494
495
496
497 ist = 0;
498 indst.push_back(-1);
499
500 while (ist <= nst)
501 {
502 if (indst[ist] == -1)
503 {
504 if (ja == -1) {ja = ist;}
505 else if (jb == 0) {jb = ist;}
506 else
507 {
508 rmeas = arest[ja];
509 wght = arest[jb];
512
513 for (i=(jb+1); i<ist; i++)
514
515 {
516 j = indst[i];
517 if (
verbose_mode) cout <<
"dparm[" << j <<
"] = " << dparm[j] << endl;
518 if (
verbose_mode) cout <<
"Starting misalignment = " << pparm[j] << endl;
519 rmeas -= arest[i]*(pparm[j]+dparm[j]);
520 }
521
522 if (
verbose_mode) cout <<
"rmeas after global stuff removal = " << rmeas << endl ;
523
524 for (i=(ja+1); i<jb; i++)
525 {
526 j = indst[i];
527 blvec[j] += wght*rmeas*arest[i];
528
529 if (
verbose_mode) cout <<
"blvec[" << j <<
"] = " << blvec[j] << endl ;
530
531 for (k=(ja+1); k<=i ; k++)
532 {
533 ik = indst[k];
534 clmat[j][ik] += wght*arest[i]*arest[k];
535
536 if (
verbose_mode) cout <<
"clmat[" << j <<
"][" << ik <<
"] = " << clmat[j][ik] << endl;
537 }
538 }
539 ja = -1;
540 jb = 0;
541 ist--;
542 }
543 }
544 ist++;
545 }
546
547
548
549
550
551
552 nrank = 0;
553 nrank = Millepede::SpmInv(clmat, blvec, nalc, scdiag, scflag);
554
556 if (
debug_mode) cout <<
" __________________________________________________" << endl;
557 if (
debug_mode) cout <<
" Printout of local fit (FITLOC) with rank= "<< nrank << endl;
558 if (
debug_mode) cout <<
" Result of local fit : (index/parameter/error)" << endl;
559
560 for (i=0; i<nalc; i++)
561 {
562 if (
debug_mode) cout << std::setprecision(4) << std::fixed;
563 if (
debug_mode) cout << std::setw(20) << i <<
" / " << std::setw(10)
564 << blvec[i] << " / " << sqrt(clmat[i][i]) << endl;
565 }
566
567
568
569
570 for (i=0; i<nalc; i++)
571 {
572 track_params[2*i] = blvec[i];
573 track_params[2*i+1] = sqrt(fabs(clmat[i][i]));
574 }
575
576
577
578
579
580
581 ist = 0;
582 ja = -1;
583 jb = 0;
584
585 while (ist <= nst)
586 {
587 if (indst[ist] == -1)
588 {
589 if (ja == -1) {ja = ist;}
590 else if (jb == 0) {jb = ist;}
591 else
592 {
593 rmeas = arest[ja];
594 wght = arest[jb];
595
596 nderlc = jb-ja-1;
597 ndergl = ist-jb-1;
598
599
600
602 if (
verbose_mode) cout << std::setprecision(4) << std::fixed;
603 if (
verbose_mode) cout <<
". equation: measured value " << std::setw(13)
604 << rmeas << " +/- " << std::setw(13) << 1.0/sqrt(wght) << endl;
605 if (
verbose_mode) cout <<
"Number of derivatives (global, local): "
606 << ndergl << ", " << nderlc << endl;
607 if (
verbose_mode) cout <<
"Global derivatives are: (index/derivative/parvalue) " << endl;
608
609 for (i=(jb+1); i<ist; i++)
610 {
if (
verbose_mode) cout << indst[i] <<
" / " << arest[i]
611 << " / " << pparm[indst[i]] << endl;}
612
613 if (
verbose_mode) cout <<
"Local derivatives are: (index/derivative) " << endl;
614
615 for (i=(ja+1); i<jb; i++) {
if (
verbose_mode) cout << indst[i] <<
" / " << arest[i] << endl;}
616
617
618
619 for (i=(ja+1); i<jb; i++)
620 {
621 j = indst[i];
622 rmeas -= arest[i]*blvec[j];
623 }
624
625 for (i=(jb+1); i<ist; i++)
626 {
627 j = indst[i];
628 rmeas -= arest[i]*(pparm[j]+dparm[j]);
629 }
630
631
632
634
635
636 if (fabs(rmeas) >= m_residual_cut_init && itert <= 1)
637 {
638
640 locrej++;
641 indst.clear();
642 arest.clear();
643 return false;
644 }
645
646 if (fabs(rmeas) >= m_residual_cut && itert > 1)
647 {
648
650 locrej++;
651 indst.clear();
652 arest.clear();
653 return false;
654 }
655
656 summ += wght*rmeas*rmeas ;
657 nsum++;
658 ja = -1;
659 jb = 0;
660 ist--;
661 }
662 }
663 ist++;
664 }
665
666 ndf = nsum-nrank;
667 rms = 0.0;
668
669 if (
debug_mode) cout <<
"Final chi square / degrees of freedom "<< summ <<
" / " << ndf << endl;
670
671 if (ndf > 0) rms = summ/float(ndf);
672
673 if (single_fit == 0) loctot++;
674
675 if (nstdev != 0 && ndf > 0 && single_fit != 1)
676 {
677 cutval = Millepede::chindl(nstdev, ndf)*cfactr;
678
679 if (
debug_mode) cout <<
"Reject if Chisq/Ndf = " << rms <<
" > " << cutval << endl;
680
681 if (rms > cutval)
682 {
683 if (
verbose_mode) cout <<
"Rejected track !!!!!" << endl;
684 if (single_fit == 0) locrej++;
685 indst.clear();
686 arest.clear();
687 return false;
688 }
689 }
690
691 if (single_fit == 1)
692 {
693 indst.clear();
694 arest.clear();
695 return true;
696 }
697
698
699
700
701
702
703 ist = 0;
704 ja = -1;
705 jb = 0;
706
707 while (ist <= nst)
708 {
709 if (indst[ist] == -1)
710 {
711 if (ja == -1) {ja = ist;}
712 else if (jb == 0) {jb = ist;}
713 else
714 {
715 rmeas = arest[ja];
716 wght = arest[jb];
717
718 for (i=(jb+1); i<ist; i++)
719 {
720 j = indst[i];
721 rmeas -= arest[i]*(pparm[j]+dparm[j]);
722 }
723
724
725
726 for (i=(jb+1); i<ist; i++)
727 {
728 j = indst[i];
729
730 bgvec[j] += wght*rmeas*arest[i];
731 if (
verbose_mode) cout <<
"bgvec[" << j <<
"] = " << bgvec[j] << endl ;
732
733 for (k=(jb+1); k<ist ; k++)
734 {
735 ik = indst[k];
736 cgmat[j][ik] += wght*arest[i]*arest[k];
737 if (
verbose_mode) cout <<
"cgmat[" << j <<
"][" << ik <<
"] = " << cgmat[j][ik] << endl;
738 }
739 }
740
741
742
743 for (i=(jb+1); i<ist; i++)
744 {
745 j = indst[i];
746 ik = indnz[j];
747
748 if (ik == -1)
749 {
750 for (k=0; k<nalc; k++) {clcmat[nagbn][k] = 0.0;}
751
752 indnz[j] = nagbn;
753 indbk[nagbn] = j;
754 ik = nagbn;
755 nagbn++;
756 }
757
758 for (k=(ja+1); k<jb ; k++)
759 {
760 ij = indst[k];
761 clcmat[ik][ij] += wght*arest[i]*arest[k];
762 if (
verbose_mode) cout <<
"clcmat[" << ik <<
"][" << ij <<
"] = " << clcmat[ik][ij] << endl;
763 }
764 }
765 ja = -1;
766 jb = 0;
767 ist--;
768 }
769 }
770 ist++;
771 }
772
773
774
775 Millepede::SpAVAt(clmat, clcmat, corrm, nalc, nagbn);
776 Millepede::SpAX(clcmat, blvec, corrv, nalc, nagbn);
777
778 for (i=0; i<nagbn; i++)
779 {
780 j = indbk[i];
781 bgvec[j] -= corrv[i];
782
783 for (k=0; k<nagbn; k++)
784 {
785 ik = indbk[k];
786 cgmat[j][ik] -= corrm[i][k];
787 }
788 }
789
790 indst.clear();
791 arest.clear();
792
793 return true;
794}
const bool verbose_reject