552 {
553
554 Amg::Transform3D globalFrameToAlignFrame =
module->globalFrameToAlignFrame();
557 globalFrameToAlignFrame(0,1)<<" "<<
558 globalFrameToAlignFrame(0,2));
560 globalFrameToAlignFrame(1,1)<<" "<<
561 globalFrameToAlignFrame(1,2));
563 globalFrameToAlignFrame(2,1)<<" "<<
564 globalFrameToAlignFrame(2,2));
565
569 globalToAlignFrameRotation(0,1)<<" "<<
570 globalToAlignFrameRotation(0,2));
572 globalToAlignFrameRotation(1,1)<<" "<<
573 globalToAlignFrameRotation(1,2));
575 globalToAlignFrameRotation(2,1)<<" "<<
576 globalToAlignFrameRotation(2,2));
577
579 const int nAlignPar = alignPars->
size();
580
581
582 std::vector<Amg::VectorX> derivatives( nAlignPar+3 ,
Amg::VectorX(alignTrack->nAlignTSOSMeas()));
583 for(
int i(0);
i<nAlignPar+3; ++
i) derivatives[i].setZero();
584
585 int imeas(0);
587 for (; iatsos != alignTrack->lastAtsos(); ++iatsos) {
588
589 AlignTSOS * alignTSOS = *iatsos;
590 if (!alignTSOS->isValid() || nullptr==alignTSOS->module())
591 continue;
592
593
594 int nResDim = alignTSOS->nResDim();
595 if (alignTSOS->module() != module) {
596 imeas += nResDim;
597 continue;
598 }
599
600
601 std::unique_ptr<std::vector<Amg::VectorX>> atsosDerivs;
602 std::unique_ptr<std::vector<Amg::VectorX>>atsosDerVtx;
604 atsosDerivs = std::make_unique<std::vector<Amg::VectorX>>(nResDim,
Amg::VectorX(nAlignPar));
605 atsosDerVtx = std::make_unique<std::vector<Amg::VectorX>>(nResDim,
Amg::VectorX(3));
606 ATH_MSG_DEBUG(
"nResDim = "<<nResDim<<
" vector size is "<<atsosDerivs->size());
607 ATH_MSG_DEBUG(
"nAlignPar = "<<nAlignPar<<
" CLHEP::HepVector size is "<<atsosDerivs->at(0).rows());
608 }
609
610
611
612
614 if (!mtp || !(mtp->covariance()) ){
615 continue;
616 }
618
619
622 localToGlobalRotation(0,1) << " " <<
623 localToGlobalRotation(0,2));
625 localToGlobalRotation(1,1) << " " <<
626 localToGlobalRotation(1,2));
628 localToGlobalRotation(2,1) << " " <<
629 localToGlobalRotation(2,2));
630
631 if(double alphastrip=alignTSOS->alphaStrip()) {
632 ATH_MSG_DEBUG(
"applying fanout rotation : " << alphastrip );
636 localToGlobalRotation(0,1) << " " <<
637 localToGlobalRotation(0,2));
639 localToGlobalRotation(1,1) << " " <<
640 localToGlobalRotation(1,2));
642 localToGlobalRotation(2,1) << " " <<
643 localToGlobalRotation(2,2));
644 }
645
646
647 Amg::Vector3D refPos = globalFrameToAlignFrame * alignTSOS->trackParameters()->position();
649
650
656
657
658
659
660
661
662
663
664
665
666
667
671 trkpars = alignTSOS->trackParameters();
672 }
673 else {
675 trkpars = alignTSOS->unbiasedTrackPars();
676 }
677
678
680 ATH_MSG_DEBUG(
"trackdir " << trackdir[0] <<
" " << trackdir[1] <<
" " << trackdir[2]);
681
682
684 double cotphi_x = trackdir.x() / trackdir.z();
685
686
687
688 if (alignTSOS->measDir() ==
Trk::y)
689 cotphi_x = trackdir.y() / trackdir.z();
690
691
692 double Rxx =
R(0,0) - cotphi_x *
R(0,2);
693 double Ryx =
R(1,0) - cotphi_x *
R(1,2);
694 double Rzx =
R(2,0) - cotphi_x *
R(2,2);
695 ATH_MSG_DEBUG(
"Rxx/Ryx/Rzx: " << Rxx <<
"/" << Ryx <<
"/" << Rzx);
696
707
717
726
727
729 const double localz = alignTSOS->trackParameters()->position().z();
730
731 const double z0z0 = 366.5*366.5;
732
734
735
736
738 Amg::Vector3D RxGlob=-1.0 * (globalToAlignFrameRotation.inverse() * RxLoc);
739
740 for (int ipar=0; ipar<nAlignPar; ipar++) {
741 const AlignPar * alignPar = (*alignPars)[ipar];
742 int paramType = alignPar->paramType();
743
745 derivatives[ipar][imeas] = projR[paramType];
747 (*atsosDerivs)[0][ipar] = projR[paramType];
748 }
749
750 for (int ipar=0; ipar<3; ipar++) {
751 derivatives[nAlignPar+ipar][imeas] = RxGlob[ipar];
753 }
754
755 for (
int i=0;
i<nAlignPar+3;
i++)
756 ATH_MSG_DEBUG(
"derivatives["<<i<<
"]["<<imeas<<
"]="<<derivatives[i][imeas]);
757
758 imeas++;
759
760 if (nResDim>1) {
761
762
763 double cotphi_y = trackdir.y() / trackdir.z() ;
764 double Rxy =
R(0,1) - cotphi_y *
R(0,2) ;
765 double Ryy =
R(1,1) - cotphi_y *
R(1,2) ;
766 double Rzy =
R(2,1) - cotphi_y *
R(2,2) ;
767 ATH_MSG_DEBUG(
"Rxy/Ryy/Rzy: " << Rxy <<
"/" << Ryy <<
"/" << Rzy);
768
775
776
780
781
782
784 Amg::Vector3D RyGlob=-1.0 * (globalToAlignFrameRotation.inverse() * RyLoc);
785
786 for (int ipar=0; ipar<nAlignPar; ipar++) {
787 const AlignPar * alignPar = (*alignPars)[ipar];
788 int paramType = alignPar->paramType();
789 ATH_MSG_DEBUG(
"2nd dim, ipar="<<ipar<<
", paramType="<<paramType);
790
791 derivatives[ipar][imeas] = projR[paramType];
793 (*atsosDerivs)[1][ipar] = projR[paramType];
794 }
795
796
797 for (int ipar=0; ipar<3; ipar++) {
798 derivatives[nAlignPar+ipar][imeas] = RyGlob[ipar];
800 }
801
802 for (
int i=0;
i<nAlignPar+3;
i++)
803 ATH_MSG_DEBUG(
"2nd dim: derivatives["<<i<<
"]["<<imeas<<
"]="<<derivatives[i][imeas]);
804
805 imeas++;
806 }
807
808 alignTSOS->setDerivatives(atsosDerivs.release());
809 alignTSOS->setDerivativesVtx(atsosDerVtx.release());
810 }
812 return derivatives;
813 }
DataModel_detail::iterator< DataVector > iterator
size_type size() const noexcept
Returns the number of elements in the collection.
const Amg::Vector3D & momentum() const
Access method for the momentum.
Eigen::AngleAxisd AngleAxis3D
Eigen::Matrix< double, 3, 3 > RotationMatrix3D
Eigen::Affine3d Transform3D
Eigen::Matrix< double, 3, 1 > Vector3D
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorX
Dynamic Vector - dynamic allocation.
ParametersBase< TrackParametersDim, Charged > TrackParameters