553 {
554
555 Amg::Transform3D globalFrameToAlignFrame =
module->globalFrameToAlignFrame();
558 globalFrameToAlignFrame(0,1)<<" "<<
559 globalFrameToAlignFrame(0,2));
561 globalFrameToAlignFrame(1,1)<<" "<<
562 globalFrameToAlignFrame(1,2));
564 globalFrameToAlignFrame(2,1)<<" "<<
565 globalFrameToAlignFrame(2,2));
566
570 globalToAlignFrameRotation(0,1)<<" "<<
571 globalToAlignFrameRotation(0,2));
573 globalToAlignFrameRotation(1,1)<<" "<<
574 globalToAlignFrameRotation(1,2));
576 globalToAlignFrameRotation(2,1)<<" "<<
577 globalToAlignFrameRotation(2,2));
578
580 const int nAlignPar = alignPars->
size();
581
582
583 std::vector<Amg::VectorX> derivatives( nAlignPar+3 ,
Amg::VectorX(alignTrack->nAlignTSOSMeas()));
584 for(
int i(0);
i<nAlignPar+3; ++
i) derivatives[i].setZero();
585
586 int imeas(0);
588 for (; iatsos != alignTrack->lastAtsos(); ++iatsos) {
589
590 AlignTSOS * alignTSOS = *iatsos;
591 if (!alignTSOS->isValid() || nullptr==alignTSOS->module())
592 continue;
593
594
595 int nResDim = alignTSOS->nResDim();
596 if (alignTSOS->module() != module) {
597 imeas += nResDim;
598 continue;
599 }
600
601
602 std::vector<Amg::VectorX> * atsosDerivs = nullptr;
603 std::vector<Amg::VectorX> * atsosDerVtx = nullptr;
605 atsosDerivs =
new std::vector<Amg::VectorX>(nResDim,
Amg::VectorX(nAlignPar));
606 atsosDerVtx =
new std::vector<Amg::VectorX>(nResDim,
Amg::VectorX(3));
607 ATH_MSG_DEBUG(
"nResDim = "<<nResDim<<
" vector size is "<<atsosDerivs->size());
608 ATH_MSG_DEBUG(
"nAlignPar = "<<nAlignPar<<
" CLHEP::HepVector size is "<<atsosDerivs->at(0).rows());
609 }
610
611
612
613
615 if (!mtp || !(mtp->covariance()) ) continue;
617
618
621 localToGlobalRotation(0,1) << " " <<
622 localToGlobalRotation(0,2));
624 localToGlobalRotation(1,1) << " " <<
625 localToGlobalRotation(1,2));
627 localToGlobalRotation(2,1) << " " <<
628 localToGlobalRotation(2,2));
629
630 if(double alphastrip=alignTSOS->alphaStrip()) {
631 ATH_MSG_DEBUG(
"applying fanout rotation : " << alphastrip );
635 localToGlobalRotation(0,1) << " " <<
636 localToGlobalRotation(0,2));
638 localToGlobalRotation(1,1) << " " <<
639 localToGlobalRotation(1,2));
641 localToGlobalRotation(2,1) << " " <<
642 localToGlobalRotation(2,2));
643 }
644
645
646 Amg::Vector3D refPos = globalFrameToAlignFrame * alignTSOS->trackParameters()->position();
648
649
655
656
657
658
659
660
661
662
663
664
665
666
670 trkpars = alignTSOS->trackParameters();
671 }
672 else {
674 trkpars = alignTSOS->unbiasedTrackPars();
675 }
676
677
679 ATH_MSG_DEBUG(
"trackdir " << trackdir[0] <<
" " << trackdir[1] <<
" " << trackdir[2]);
680
681
683 double cotphi_x = trackdir.x() / trackdir.z();
684
685
686
687 if (alignTSOS->measDir() ==
Trk::y)
688 cotphi_x = trackdir.y() / trackdir.z();
689
690
691 double Rxx =
R(0,0) - cotphi_x *
R(0,2);
692 double Ryx =
R(1,0) - cotphi_x *
R(1,2);
693 double Rzx =
R(2,0) - cotphi_x *
R(2,2);
694 ATH_MSG_DEBUG(
"Rxx/Ryx/Rzx: " << Rxx <<
"/" << Ryx <<
"/" << Rzx);
695
706
716
725
726
728 const double localz = alignTSOS->trackParameters()->position().z();
729
730 const double z0z0 = 366.5*366.5;
731
733
734
735
737 Amg::Vector3D RxGlob=-1.0 * (globalToAlignFrameRotation.inverse() * RxLoc);
738
739 for (int ipar=0; ipar<nAlignPar; ipar++) {
740 const AlignPar * alignPar = (*alignPars)[ipar];
741 int paramType = alignPar->paramType();
742
744 derivatives[ipar][imeas] = projR[paramType];
746 (*atsosDerivs)[0][ipar] = projR[paramType];
747 }
748
749 for (int ipar=0; ipar<3; ipar++) {
750 derivatives[nAlignPar+ipar][imeas] = RxGlob[ipar];
752 }
753
754 for (
int i=0;
i<nAlignPar+3;
i++)
755 ATH_MSG_DEBUG(
"derivatives["<<i<<
"]["<<imeas<<
"]="<<derivatives[i][imeas]);
756
757 imeas++;
758
759 if (nResDim>1) {
760
761
762 double cotphi_y = trackdir.y() / trackdir.z() ;
763 double Rxy =
R(0,1) - cotphi_y *
R(0,2) ;
764 double Ryy =
R(1,1) - cotphi_y *
R(1,2) ;
765 double Rzy =
R(2,1) - cotphi_y *
R(2,2) ;
766 ATH_MSG_DEBUG(
"Rxy/Ryy/Rzy: " << Rxy <<
"/" << Ryy <<
"/" << Rzy);
767
774
775
779
780
781
783 Amg::Vector3D RyGlob=-1.0 * (globalToAlignFrameRotation.inverse() * RyLoc);
784
785 for (int ipar=0; ipar<nAlignPar; ipar++) {
786 const AlignPar * alignPar = (*alignPars)[ipar];
787 int paramType = alignPar->paramType();
788 ATH_MSG_DEBUG(
"2nd dim, ipar="<<ipar<<
", paramType="<<paramType);
789
790 derivatives[ipar][imeas] = projR[paramType];
792 (*atsosDerivs)[1][ipar] = projR[paramType];
793 }
794
795
796 for (int ipar=0; ipar<3; ipar++) {
797 derivatives[nAlignPar+ipar][imeas] = RyGlob[ipar];
799 }
800
801 for (
int i=0;
i<nAlignPar+3;
i++)
802 ATH_MSG_DEBUG(
"2nd dim: derivatives["<<i<<
"]["<<imeas<<
"]="<<derivatives[i][imeas]);
803
804 imeas++;
805 }
806
807 alignTSOS->setDerivatives(atsosDerivs);
808 alignTSOS->setDerivativesVtx(atsosDerVtx);
809 }
811 return derivatives;
812 }
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