Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2024-04-06 11:57:24 

0001 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImage.h"
0002 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImageLocalFit.h"
0003 
0004 #include <stdexcept>
0005 #include <utility>
0006 #include <sstream>
0007 #include <vector>
0008 #include <cmath>
0009 #include "DataFormats/GeometryVector/interface/LocalPoint.h"
0010 #include "Math/SMatrix.h"
0011 #include "Math/SVector.h"
0012 
0013 #include <iostream>
0014 
0015 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec,
0016                                    const pede_label_t &label1,
0017                                    const pede_label_t &label2) {
0018   labelVec1_.clear();
0019   labelVec1_.push_back(label1 + 0);
0020   labelVec1_.push_back(label1 + 1);
0021   labelVec1_.push_back(label1 + 5);
0022   labelVec2_.clear();
0023   labelVec2_.push_back(label2 + 0);
0024   labelVec2_.push_back(label2 + 1);
0025   labelVec2_.push_back(label2 + 5);
0026   doFit(fidpointvec);
0027 }
0028 
0029 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec) {
0030   fitValidFlag_ = false;
0031 
0032   // Calculate gamma of right module w.r.t left modules' fram
0033   const value_t dxr = fidpointvec[3].x() - fidpointvec[2].x();
0034   const value_t dyr = fidpointvec[3].y() - fidpointvec[2].y();
0035   const value_t gammar = atan(dyr / dxr);
0036   const value_t dxl = fidpointvec[1].x() - fidpointvec[0].x();
0037   const value_t dyl = fidpointvec[1].y() - fidpointvec[0].y();
0038   const value_t gammal = atan(dyl / dxl);
0039   const value_t gamma = gammar - gammal;
0040   //const value_t gamma = 0.; // Testhalber
0041   const value_t sing = sin(gamma);
0042   const value_t cosg = cos(gamma);
0043 #ifdef DEBUG
0044   std::cout << "gamma: " << gamma << " gamma left: " << gammal << " gamma right: " << gammar << std::endl;
0045 #endif
0046 
0047 #ifdef DEBUG
0048   std::cout << "&fidpointvec: " << std::endl;
0049   for (count_t i = 0; i != fidpointvec.size(); i++)
0050     std::cout << i << ": " << fidpointvec[i] << std::endl;
0051   std::cout << "&fidpoints_: " << std::endl;
0052   for (count_t i = 0; i != fidpoints_.size(); i++)
0053     std::cout << i << ": " << fidpoints_[i] << std::endl;
0054 #endif
0055 
0056   // Matrix of the local derivatives
0057   ROOT::Math::SMatrix<value_t, nMsrmts, nLcD> A;  // 8x4
0058   A(0, 0) = 1.;
0059   A(0, 1) = 0;
0060   A(0, 2) = +fidpointvec[0].x();
0061   A(0, 3) = +fidpointvec[0].y();
0062   A(1, 0) = 0.;
0063   A(1, 1) = 1;
0064   A(1, 2) = +fidpointvec[0].y();
0065   A(1, 3) = -fidpointvec[0].x();
0066   A(2, 0) = 1.;
0067   A(2, 1) = 0;
0068   A(2, 2) = +fidpointvec[1].x();
0069   A(2, 3) = +fidpointvec[1].y();
0070   A(3, 0) = 0.;
0071   A(3, 1) = 1;
0072   A(3, 2) = +fidpointvec[1].y();
0073   A(3, 3) = -fidpointvec[1].x();
0074   A(4, 0) = 1.;
0075   A(4, 1) = 0;
0076   A(4, 2) = +fidpointvec[2].x();
0077   A(4, 3) = +fidpointvec[2].y();
0078   A(5, 0) = 0.;
0079   A(5, 1) = 1;
0080   A(5, 2) = +fidpointvec[2].y();
0081   A(5, 3) = -fidpointvec[2].x();
0082   A(6, 0) = 1.;
0083   A(6, 1) = 0;
0084   A(6, 2) = +fidpointvec[3].x();
0085   A(6, 3) = +fidpointvec[3].y();
0086   A(7, 0) = 0.;
0087   A(7, 1) = 1;
0088   A(7, 2) = +fidpointvec[3].y();
0089   A(7, 3) = -fidpointvec[3].x();
0090 #ifdef DEBUG
0091   std::cout << "A: \n" << A << std::endl;
0092 #endif
0093 
0094   // Covariance matrix
0095   ROOT::Math::SMatrix<value_t, nMsrmts, nMsrmts> W;  // 8x8
0096   //ROOT::Math::MatRepSym<value_t,8> W;
0097   const value_t sigma_x2inv = 1. / (sigma_x_ * sigma_x_);
0098   const value_t sigma_y2inv = 1. / (sigma_y_ * sigma_y_);
0099   W(0, 0) = sigma_x2inv;
0100   W(1, 1) = sigma_y2inv;
0101   W(2, 2) = sigma_x2inv;
0102   W(3, 3) = sigma_y2inv;
0103   W(4, 4) = sigma_x2inv;
0104   W(5, 5) = sigma_y2inv;
0105   W(6, 6) = sigma_x2inv;
0106   W(7, 7) = sigma_y2inv;
0107 #ifdef DEBUG
0108   std::cout << "W: \n" << W << std::endl;
0109 #endif
0110 
0111   // Prepare for the fit
0112   ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWA;  // 4x4
0113   ATWA = ROOT::Math::Transpose(A) * W * A;
0114   //ATWA = ROOT::Math::SimilarityT(A,W); // W muss symmterisch sein -> aendern.
0115   //std::cout << "ATWA: \n" << ATWA << std::endl;
0116   ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWAi;  // 4x4
0117   int ifail = 0;
0118   ATWAi = ATWA.Inverse(ifail);
0119   if (ifail != 0) {  // TODO: ifail Pruefung auf message logger ausgeben statt cout
0120     std::cout << "Problem singular - fit impossible." << std::endl;
0121     fitValidFlag_ = false;
0122     return;
0123   }
0124 #ifdef DEBUG
0125   std::cout << "ATWA-1: \n" << ATWAi << ifail << std::endl;
0126 #endif
0127 
0128   // Measurements
0129   ROOT::Math::SVector<value_t, nMsrmts> y;  // 8
0130   y(0) = measurementVec_[0].x();
0131   y(1) = measurementVec_[0].y();
0132   y(2) = measurementVec_[1].x();
0133   y(3) = measurementVec_[1].y();
0134   y(4) = measurementVec_[2].x();
0135   y(5) = measurementVec_[2].y();
0136   y(6) = measurementVec_[3].x();
0137   y(7) = measurementVec_[3].y();
0138 #ifdef DEBUG
0139   std::cout << "y: " << y << std::endl;
0140 #endif
0141 
0142   // do the fit
0143   ROOT::Math::SVector<value_t, nLcD> a;  // 4
0144   a = ATWAi * ROOT::Math::Transpose(A) * W * y;
0145   chi2_ = ROOT::Math::Dot(y, W * y) - ROOT::Math::Dot(a, ROOT::Math::Transpose(A) * W * y);
0146 #ifdef DEBUG
0147   std::cout << "a: " << a << " S= " << sqrt(a[2] * a[2] + a[3] * a[3]) << " phi= " << atan(a[3] / a[2])
0148             << " chi2= " << chi2_ << std::endl;
0149   std::cout << "A*a: " << A * a << std::endl;
0150 #endif
0151   a_.assign(a.begin(), a.end());
0152 
0153   // Calculate vector of residuals
0154   r = y - A * a;
0155 #ifdef DEBUG
0156   std::cout << "r: " << r << std::endl;
0157 #endif
0158 
0159   // Fill matrix for global fit with local derivatives
0160   localDerivsMatrix_(0, 0) = 1.;
0161   localDerivsMatrix_(0, 1) = 0;
0162   localDerivsMatrix_(1, 0) = 0.;
0163   localDerivsMatrix_(1, 1) = 1;
0164   localDerivsMatrix_(2, 0) = 1.;
0165   localDerivsMatrix_(2, 1) = 0;
0166   localDerivsMatrix_(3, 0) = 0.;
0167   localDerivsMatrix_(3, 1) = 1;
0168   localDerivsMatrix_(4, 0) = 1.;
0169   localDerivsMatrix_(4, 1) = 0;
0170   localDerivsMatrix_(5, 0) = 0.;
0171   localDerivsMatrix_(5, 1) = 1;
0172   localDerivsMatrix_(6, 0) = 1.;
0173   localDerivsMatrix_(6, 1) = 0;
0174   localDerivsMatrix_(7, 0) = 0.;
0175   localDerivsMatrix_(7, 1) = 1;
0176   localDerivsMatrix_(0, 2) = +fidpointvec[0].x() + cosg * fidpoints_[0].x() - sing * fidpoints_[0].y();
0177   localDerivsMatrix_(0, 3) = +fidpointvec[0].y() + cosg * fidpoints_[0].y() + sing * fidpoints_[0].x();
0178   localDerivsMatrix_(1, 2) = +fidpointvec[0].y() + cosg * fidpoints_[0].y() + sing * fidpoints_[0].x();
0179   localDerivsMatrix_(1, 3) = -fidpointvec[0].x() - cosg * fidpoints_[0].x() + sing * fidpoints_[0].y();
0180   localDerivsMatrix_(2, 2) = +fidpointvec[1].x() + cosg * fidpoints_[1].x() - sing * fidpoints_[1].y();
0181   localDerivsMatrix_(2, 3) = +fidpointvec[1].y() + cosg * fidpoints_[1].y() + sing * fidpoints_[1].x();
0182   localDerivsMatrix_(3, 2) = +fidpointvec[1].y() + cosg * fidpoints_[1].y() + sing * fidpoints_[1].x();
0183   localDerivsMatrix_(3, 3) = -fidpointvec[1].x() - cosg * fidpoints_[1].x() + sing * fidpoints_[1].y();
0184   localDerivsMatrix_(4, 2) = +fidpointvec[2].x() + cosg * fidpoints_[2].x() - sing * fidpoints_[2].y();
0185   localDerivsMatrix_(4, 3) = +fidpointvec[2].y() + cosg * fidpoints_[2].y() + sing * fidpoints_[2].x();
0186   localDerivsMatrix_(5, 2) = +fidpointvec[2].y() + cosg * fidpoints_[2].y() + sing * fidpoints_[2].x();
0187   localDerivsMatrix_(5, 3) = -fidpointvec[2].x() - cosg * fidpoints_[2].x() + sing * fidpoints_[2].y();
0188   localDerivsMatrix_(6, 2) = +fidpointvec[3].x() + cosg * fidpoints_[3].x() - sing * fidpoints_[3].y();
0189   localDerivsMatrix_(6, 3) = +fidpointvec[3].y() + cosg * fidpoints_[3].y() + sing * fidpoints_[3].x();
0190   localDerivsMatrix_(7, 2) = +fidpointvec[3].y() + cosg * fidpoints_[3].y() + sing * fidpoints_[3].x();
0191   localDerivsMatrix_(7, 3) = -fidpointvec[3].x() - cosg * fidpoints_[3].x() + sing * fidpoints_[3].y();
0192 
0193   // Fill vector with global derivatives and labels (8x3)
0194   globalDerivsMatrix_(0, 0) = +a(2);
0195   globalDerivsMatrix_(0, 1) = +a(3);
0196   globalDerivsMatrix_(0, 2) = +cosg * (a(3) * fidpoints_[0].x() - a(2) * fidpoints_[0].y()) -
0197                               sing * (a(2) * fidpoints_[0].x() + a(3) * fidpoints_[0].y());
0198   globalDerivsMatrix_(1, 0) = -a(3);
0199   globalDerivsMatrix_(1, 1) = +a(2);
0200   globalDerivsMatrix_(1, 2) = +cosg * (a(2) * fidpoints_[0].x() + a(3) * fidpoints_[0].y()) -
0201                               sing * (a(2) * fidpoints_[0].y() - a(3) * fidpoints_[0].x());
0202   globalDerivsMatrix_(2, 0) = +a(2);
0203   globalDerivsMatrix_(2, 1) = +a(3);
0204   globalDerivsMatrix_(2, 2) = +cosg * (a(3) * fidpoints_[1].x() - a(2) * fidpoints_[1].y()) -
0205                               sing * (a(2) * fidpoints_[1].x() + a(3) * fidpoints_[1].y());
0206   globalDerivsMatrix_(3, 0) = -a(3);
0207   globalDerivsMatrix_(3, 1) = +a(2);
0208   globalDerivsMatrix_(3, 2) = +cosg * (a(2) * fidpoints_[1].x() + a(3) * fidpoints_[1].y()) -
0209                               sing * (a(2) * fidpoints_[1].y() - a(3) * fidpoints_[1].x());
0210 
0211   globalDerivsMatrix_(4, 0) = +a(2);
0212   globalDerivsMatrix_(4, 1) = +a(3);
0213   globalDerivsMatrix_(4, 2) = +cosg * (a(3) * fidpoints_[2].x() - a(2) * fidpoints_[2].y()) -
0214                               sing * (a(2) * fidpoints_[2].x() + a(3) * fidpoints_[2].y());
0215   globalDerivsMatrix_(5, 0) = -a(3);
0216   globalDerivsMatrix_(5, 1) = +a(2);
0217   globalDerivsMatrix_(5, 2) = +cosg * (a(2) * fidpoints_[2].x() + a(3) * fidpoints_[2].y()) -
0218                               sing * (a(2) * fidpoints_[2].y() - a(3) * fidpoints_[2].x());
0219   globalDerivsMatrix_(6, 0) = +a(2);
0220   globalDerivsMatrix_(6, 1) = +a(3);
0221   globalDerivsMatrix_(6, 2) = +cosg * (a(3) * fidpoints_[3].x() - a(2) * fidpoints_[3].y()) -
0222                               sing * (a(2) * fidpoints_[3].x() + a(3) * fidpoints_[3].y());
0223   globalDerivsMatrix_(7, 0) = -a(3);
0224   globalDerivsMatrix_(7, 1) = +a(2);
0225   globalDerivsMatrix_(7, 2) = +cosg * (a(2) * fidpoints_[3].x() + a(3) * fidpoints_[3].y()) -
0226                               sing * (a(2) * fidpoints_[3].y() - a(3) * fidpoints_[3].x());
0227 
0228   fitValidFlag_ = true;
0229 }
0230 
0231 void SurveyPxbImageLocalFit::doFit(value_t u1, value_t v1, value_t g1, value_t u2, value_t v2, value_t g2) {
0232   // Creating vectors with the global parameters of the two modules
0233   ROOT::Math::SVector<value_t, 4> mod1, mod2;
0234   mod1(0) = u1;
0235   mod1(1) = v1;
0236   mod1(2) = cos(g1);
0237   mod1(3) = sin(g1);
0238   mod2(0) = u2;
0239   mod2(1) = v2;
0240   mod2(2) = cos(g2);
0241   mod2(3) = sin(g2);
0242   //std::cout << "mod1: " << mod1 << std::endl;
0243   //std::cout << "mod2: " << mod2 << std::endl;
0244 
0245   // Create a matrix for the transformed position of the fidpoints
0246   ROOT::Math::SMatrix<value_t, 4, 4> M1, M2;
0247   M1(0, 0) = 1.;
0248   M1(0, 1) = 0.;
0249   M1(0, 2) = +fidpoints_[0].x();
0250   M1(0, 3) = -fidpoints_[0].y();
0251   M1(1, 0) = 0.;
0252   M1(1, 1) = 1.;
0253   M1(1, 2) = +fidpoints_[0].y();
0254   M1(1, 3) = +fidpoints_[0].x();
0255   M1(2, 0) = 1.;
0256   M1(2, 1) = 0.;
0257   M1(2, 2) = +fidpoints_[1].x();
0258   M1(2, 3) = -fidpoints_[1].y();
0259   M1(3, 0) = 0.;
0260   M1(3, 1) = 1.;
0261   M1(3, 2) = +fidpoints_[1].y();
0262   M1(3, 3) = +fidpoints_[1].x();
0263   M2(0, 0) = 1.;
0264   M2(0, 1) = 0.;
0265   M2(0, 2) = +fidpoints_[2].x();
0266   M2(0, 3) = -fidpoints_[2].y();
0267   M2(1, 0) = 0.;
0268   M2(1, 1) = 1.;
0269   M2(1, 2) = +fidpoints_[2].y();
0270   M2(1, 3) = +fidpoints_[2].x();
0271   M2(2, 0) = 1.;
0272   M2(2, 1) = 0.;
0273   M2(2, 2) = +fidpoints_[3].x();
0274   M2(2, 3) = -fidpoints_[3].y();
0275   M2(3, 0) = 0.;
0276   M2(3, 1) = 1.;
0277   M2(3, 2) = +fidpoints_[3].y();
0278   M2(3, 3) = +fidpoints_[3].x();
0279 
0280   //std::cout << "M1:\n" << M1 << std::endl;
0281   //std::cout << "M2:\n" << M2 << std::endl;
0282 
0283   ROOT::Math::SVector<value_t, 4> mod_tr1, mod_tr2;
0284   mod_tr1 = M1 * mod2;
0285   mod_tr2 = M2 * mod1;
0286   //std::cout << "mod_tr1: " << mod_tr1 << std::endl;
0287   //std::cout << "mod_tr2: " << mod_tr2 << std::endl;
0288 
0289   fidpoint_t fidpointvec;
0290   fidpointvec.push_back(coord_t(mod_tr1(0), mod_tr1(1)));
0291   fidpointvec.push_back(coord_t(mod_tr1(2), mod_tr1(3)));
0292   fidpointvec.push_back(coord_t(mod_tr2(0), mod_tr2(1)));
0293   fidpointvec.push_back(coord_t(mod_tr2(2), mod_tr2(3)));
0294 
0295   doFit(fidpointvec);
0296 }
0297 
0298 SurveyPxbImageLocalFit::localpars_t SurveyPxbImageLocalFit::getLocalParameters() {
0299   if (!fitValidFlag_)
0300     throw std::logic_error(
0301         "SurveyPxbImageLocalFit::getLocalParameters(): Fit is not valid. Call doFit(...) before calling this "
0302         "function.");
0303   return a_;
0304 }
0305 
0306 SurveyPxbImageLocalFit::value_t SurveyPxbImageLocalFit::getChi2() {
0307   if (!fitValidFlag_)
0308     throw std::logic_error(
0309         "SurveyPxbImageLocalFit::getChi2(): Fit is not valid. Call doFit(...) before calling this function.");
0310   return chi2_;
0311 }
0312 
0313 void SurveyPxbImageLocalFit::setLocalDerivsToZero(count_t j) {
0314   if (!(j < nLcD))
0315     throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
0316   for (count_t i = 0; i != nMsrmts; i++)
0317     localDerivsMatrix_(i, j) = 0;
0318 }
0319 
0320 void SurveyPxbImageLocalFit::setGlobalDerivsToZero(count_t j) {
0321   if (!(j < nGlD))
0322     throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
0323   for (count_t i = 0; i != nMsrmts; i++)
0324     globalDerivsMatrix_(i, j) = 0;
0325 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.2.1 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!