Reference Manual

Go to the documentation of this file.
00001 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImage.h"
00002 #include "Alignment/SurveyAnalysis/interface/SurveyPxbImageLocalFit.h"
00003 
00004 #include <stdexcept>
00005 #include <utility>
00006 #include <sstream>
00007 #include <vector>
00008 #include <cmath>
00009 #include "DataFormats/GeometryVector/interface/LocalPoint.h"
00010 #include "Math/SMatrix.h"
00011 #include "Math/SVector.h"
00012 
00013 #include <iostream>
00014 
00015 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec, const pede_label_t &label1, const pede_label_t &label2)
00016 {
00017         labelVec1_.clear();
00018         labelVec1_.push_back(label1+0);
00019         labelVec1_.push_back(label1+1);
00020         labelVec1_.push_back(label1+5);
00021         labelVec2_.clear();
00022         labelVec2_.push_back(label2+0);
00023         labelVec2_.push_back(label2+1);
00024         labelVec2_.push_back(label2+5);
00025         doFit(fidpointvec);
00026 }
00027 
00028 void SurveyPxbImageLocalFit::doFit(const fidpoint_t &fidpointvec)
00029 {
00030         fitValidFlag_ = false;
00031 
00032         // Calculate gamma of right module w.r.t left modules' fram
00033         const value_t dxr = fidpointvec[3].x()-fidpointvec[2].x();
00034         const value_t dyr = fidpointvec[3].y()-fidpointvec[2].y();
00035         const value_t gammar = atan(dyr/dxr);
00036         const value_t dxl = fidpointvec[1].x()-fidpointvec[0].x();
00037         const value_t dyl = fidpointvec[1].y()-fidpointvec[0].y();
00038         const value_t gammal = atan(dyl/dxl);
00039         const value_t gamma = gammar-gammal;
00040         //const value_t gamma = 0.; // Testhalber
00041         const value_t sing = sin(gamma);
00042         const value_t cosg = cos(gamma);
00043 #ifdef DEBUG
00044         std::cout << "gamma: " << gamma << " gamma left: " << gammal << " gamma right: " << gammar << std::endl;
00045 #endif
00046 
00047 
00048 #ifdef DEBUG
00049         std::cout << "&fidpointvec: " << std::endl;
00050         for (count_t i=0; i!=fidpointvec.size(); i++)
00051             std::cout << i << ": " << fidpointvec[i] << std::endl;
00052         std::cout << "&fidpoints_: " << std::endl;
00053         for (count_t i=0; i!=fidpoints_.size(); i++)
00054             std::cout << i << ": " << fidpoints_[i] << std::endl;
00055 #endif
00056 
00057         // Matrix of the local derivatives
00058         ROOT::Math::SMatrix<value_t,nMsrmts,nLcD> A; // 8x4
00059         A(0,0)=1.; A(0,1)=0; A(0,2)=+fidpointvec[0].x(); A(0,3)=+fidpointvec[0].y();
00060         A(1,0)=0.; A(1,1)=1; A(1,2)=+fidpointvec[0].y(); A(1,3)=-fidpointvec[0].x();
00061         A(2,0)=1.; A(2,1)=0; A(2,2)=+fidpointvec[1].x(); A(2,3)=+fidpointvec[1].y();
00062         A(3,0)=0.; A(3,1)=1; A(3,2)=+fidpointvec[1].y(); A(3,3)=-fidpointvec[1].x();
00063         A(4,0)=1.; A(4,1)=0; A(4,2)=+fidpointvec[2].x(); A(4,3)=+fidpointvec[2].y();
00064         A(5,0)=0.; A(5,1)=1; A(5,2)=+fidpointvec[2].y(); A(5,3)=-fidpointvec[2].x();
00065         A(6,0)=1.; A(6,1)=0; A(6,2)=+fidpointvec[3].x(); A(6,3)=+fidpointvec[3].y();
00066         A(7,0)=0.; A(7,1)=1; A(7,2)=+fidpointvec[3].y(); A(7,3)=-fidpointvec[3].x();
00067 #ifdef DEBUG
00068         std::cout << "A: \n" << A << std::endl;
00069 #endif
00070 
00071         // Covariance matrix
00072         ROOT::Math::SMatrix<value_t,nMsrmts,nMsrmts> W; // 8x8
00073         //ROOT::Math::MatRepSym<value_t,8> W;
00074         const value_t sigma_x2inv = 1./(sigma_x_*sigma_x_);
00075         const value_t sigma_y2inv = 1./(sigma_y_*sigma_y_);
00076         W(0,0) = sigma_x2inv;
00077         W(1,1) = sigma_y2inv;
00078         W(2,2) = sigma_x2inv;
00079         W(3,3) = sigma_y2inv;
00080         W(4,4) = sigma_x2inv;
00081         W(5,5) = sigma_y2inv;
00082         W(6,6) = sigma_x2inv;
00083         W(7,7) = sigma_y2inv;
00084 #ifdef DEBUG
00085         std::cout << "W: \n" << W << std::endl;
00086 #endif
00087 
00088         // Prepare for the fit
00089         ROOT::Math::SMatrix<value_t,nLcD,nLcD> ATWA; // 4x4
00090         ATWA = ROOT::Math::Transpose(A) * W * A;
00091         //ATWA = ROOT::Math::SimilarityT(A,W); // W muss symmterisch sein -> aendern.
00092         //std::cout << "ATWA: \n" << ATWA << std::endl;
00093         ROOT::Math::SMatrix<value_t,nLcD,nLcD> ATWAi; // 4x4
00094         int ifail = 0;
00095         ATWAi = ATWA.Inverse(ifail);
00096         if (ifail != 0)
00097         { // TODO: ifail Pruefung auf message logger ausgeben statt cout
00098                 std::cout << "Problem singular - fit impossible." << std::endl;
00099                 fitValidFlag_ = false;
00100                 return;
00101         }
00102 #ifdef DEBUG
00103         std::cout << "ATWA-1: \n" << ATWAi << ifail << std::endl;
00104 #endif
00105 
00106         // Measurements
00107         ROOT::Math::SVector<value_t,nMsrmts> y; // 8
00108         y(0) = measurementVec_[0].x();
00109         y(1) = measurementVec_[0].y();
00110         y(2) = measurementVec_[1].x();
00111         y(3) = measurementVec_[1].y();
00112         y(4) = measurementVec_[2].x();
00113         y(5) = measurementVec_[2].y();
00114         y(6) = measurementVec_[3].x();
00115         y(7) = measurementVec_[3].y();
00116 #ifdef DEBUG
00117         std::cout << "y: " << y << std::endl;
00118 #endif
00119 
00120         // do the fit
00121         ROOT::Math::SVector<value_t,nLcD> a; // 4
00122         a = ATWAi * ROOT::Math::Transpose(A) * W * y;
00123         chi2_ = ROOT::Math::Dot(y,W*y)-ROOT::Math::Dot(a,ROOT::Math::Transpose(A)*W*y);
00124 #ifdef DEBUG
00125         std::cout << "a: " << a 
00126                 << " S= " << sqrt(a[2]*a[2]+a[3]*a[3]) 
00127                 << " phi= " << atan(a[3]/a[2]) 
00128                 << " chi2= " << chi2_ << std::endl;
00129         std::cout << "A*a: " << A*a << std::endl;
00130 #endif
00131         a_.assign(a.begin(),a.end());
00132 
00133         // Calculate vector of residuals
00134         r = y - A*a;
00135 #ifdef DEBUG
00136         std::cout << "r: " << r << std::endl;
00137 #endif
00138 
00139         // Fill matrix for global fit with local derivatives
00140         localDerivsMatrix_(0,0)=1.; localDerivsMatrix_(0,1)=0; 
00141         localDerivsMatrix_(1,0)=0.; localDerivsMatrix_(1,1)=1; 
00142         localDerivsMatrix_(2,0)=1.; localDerivsMatrix_(2,1)=0; 
00143         localDerivsMatrix_(3,0)=0.; localDerivsMatrix_(3,1)=1; 
00144         localDerivsMatrix_(4,0)=1.; localDerivsMatrix_(4,1)=0; 
00145         localDerivsMatrix_(5,0)=0.; localDerivsMatrix_(5,1)=1; 
00146         localDerivsMatrix_(6,0)=1.; localDerivsMatrix_(6,1)=0; 
00147         localDerivsMatrix_(7,0)=0.; localDerivsMatrix_(7,1)=1; 
00148         localDerivsMatrix_(0,2)=+fidpointvec[0].x()+cosg*fidpoints_[0].x()-sing*fidpoints_[0].y();
00149         localDerivsMatrix_(0,3)=+fidpointvec[0].y()+cosg*fidpoints_[0].y()+sing*fidpoints_[0].x();
00150         localDerivsMatrix_(1,2)=+fidpointvec[0].y()+cosg*fidpoints_[0].y()+sing*fidpoints_[0].x();
00151         localDerivsMatrix_(1,3)=-fidpointvec[0].x()-cosg*fidpoints_[0].x()+sing*fidpoints_[0].y();
00152         localDerivsMatrix_(2,2)=+fidpointvec[1].x()+cosg*fidpoints_[1].x()-sing*fidpoints_[1].y();
00153         localDerivsMatrix_(2,3)=+fidpointvec[1].y()+cosg*fidpoints_[1].y()+sing*fidpoints_[1].x();
00154         localDerivsMatrix_(3,2)=+fidpointvec[1].y()+cosg*fidpoints_[1].y()+sing*fidpoints_[1].x();
00155         localDerivsMatrix_(3,3)=-fidpointvec[1].x()-cosg*fidpoints_[1].x()+sing*fidpoints_[1].y();
00156         localDerivsMatrix_(4,2)=+fidpointvec[2].x()+cosg*fidpoints_[2].x()-sing*fidpoints_[2].y();
00157         localDerivsMatrix_(4,3)=+fidpointvec[2].y()+cosg*fidpoints_[2].y()+sing*fidpoints_[2].x();
00158         localDerivsMatrix_(5,2)=+fidpointvec[2].y()+cosg*fidpoints_[2].y()+sing*fidpoints_[2].x();
00159         localDerivsMatrix_(5,3)=-fidpointvec[2].x()-cosg*fidpoints_[2].x()+sing*fidpoints_[2].y();
00160         localDerivsMatrix_(6,2)=+fidpointvec[3].x()+cosg*fidpoints_[3].x()-sing*fidpoints_[3].y();
00161         localDerivsMatrix_(6,3)=+fidpointvec[3].y()+cosg*fidpoints_[3].y()+sing*fidpoints_[3].x();
00162         localDerivsMatrix_(7,2)=+fidpointvec[3].y()+cosg*fidpoints_[3].y()+sing*fidpoints_[3].x();
00163         localDerivsMatrix_(7,3)=-fidpointvec[3].x()-cosg*fidpoints_[3].x()+sing*fidpoints_[3].y();
00164 
00165         // Fill vector with global derivatives and labels (8x3)
00166         globalDerivsMatrix_(0,0) = +a(2);
00167         globalDerivsMatrix_(0,1) = +a(3);
00168         globalDerivsMatrix_(0,2) = +cosg*(a(3)*fidpoints_[0].x()-a(2)*fidpoints_[0].y())
00169                                    -sing*(a(2)*fidpoints_[0].x()+a(3)*fidpoints_[0].y());
00170         globalDerivsMatrix_(1,0) = -a(3);
00171         globalDerivsMatrix_(1,1) = +a(2);
00172         globalDerivsMatrix_(1,2) = +cosg*(a(2)*fidpoints_[0].x()+a(3)*fidpoints_[0].y())
00173                                    -sing*(a(2)*fidpoints_[0].y()-a(3)*fidpoints_[0].x());
00174         globalDerivsMatrix_(2,0) = +a(2);
00175         globalDerivsMatrix_(2,1) = +a(3);
00176         globalDerivsMatrix_(2,2) = +cosg*(a(3)*fidpoints_[1].x()-a(2)*fidpoints_[1].y())
00177                                    -sing*(a(2)*fidpoints_[1].x()+a(3)*fidpoints_[1].y());
00178         globalDerivsMatrix_(3,0) = -a(3);
00179         globalDerivsMatrix_(3,1) = +a(2);
00180         globalDerivsMatrix_(3,2) = +cosg*(a(2)*fidpoints_[1].x()+a(3)*fidpoints_[1].y())
00181                                    -sing*(a(2)*fidpoints_[1].y()-a(3)*fidpoints_[1].x());
00182 
00183         globalDerivsMatrix_(4,0) = +a(2);
00184         globalDerivsMatrix_(4,1) = +a(3);
00185         globalDerivsMatrix_(4,2) = +cosg*(a(3)*fidpoints_[2].x()-a(2)*fidpoints_[2].y())
00186                                    -sing*(a(2)*fidpoints_[2].x()+a(3)*fidpoints_[2].y());
00187         globalDerivsMatrix_(5,0) = -a(3);
00188         globalDerivsMatrix_(5,1) = +a(2);
00189         globalDerivsMatrix_(5,2) = +cosg*(a(2)*fidpoints_[2].x()+a(3)*fidpoints_[2].y())
00190                                    -sing*(a(2)*fidpoints_[2].y()-a(3)*fidpoints_[2].x());
00191         globalDerivsMatrix_(6,0) = +a(2);
00192         globalDerivsMatrix_(6,1) = +a(3);
00193         globalDerivsMatrix_(6,2) = +cosg*(a(3)*fidpoints_[3].x()-a(2)*fidpoints_[3].y())
00194                                    -sing*(a(2)*fidpoints_[3].x()+a(3)*fidpoints_[3].y());
00195         globalDerivsMatrix_(7,0) = -a(3);
00196         globalDerivsMatrix_(7,1) = +a(2);
00197         globalDerivsMatrix_(7,2) = +cosg*(a(2)*fidpoints_[3].x()+a(3)*fidpoints_[3].y())
00198                                    -sing*(a(2)*fidpoints_[3].y()-a(3)*fidpoints_[3].x());
00199 
00200         fitValidFlag_ = true;
00201 }
00202 
00203 
00204 void SurveyPxbImageLocalFit::doFit(value_t u1, value_t v1, value_t g1, value_t u2, value_t v2, value_t g2)
00205 {
00206         // Creating vectors with the global parameters of the two modules
00207         ROOT::Math::SVector<value_t,4> mod1, mod2;
00208         mod1(0)=u1;
00209         mod1(1)=v1;
00210         mod1(2)=cos(g1);
00211         mod1(3)=sin(g1);
00212         mod2(0)=u2;
00213         mod2(1)=v2;
00214         mod2(2)=cos(g2);
00215         mod2(3)=sin(g2);
00216         //std::cout << "mod1: " << mod1 << std::endl;
00217         //std::cout << "mod2: " << mod2 << std::endl;
00218 
00219         // Create a matrix for the transformed position of the fidpoints
00220         ROOT::Math::SMatrix<value_t,4,4> M1, M2;
00221         M1(0,0)=1.; M1(0,1)=0.; M1(0,2)=+fidpoints_[0].x(); M1(0,3)=-fidpoints_[0].y();
00222         M1(1,0)=0.; M1(1,1)=1.; M1(1,2)=+fidpoints_[0].y(); M1(1,3)=+fidpoints_[0].x();
00223         M1(2,0)=1.; M1(2,1)=0.; M1(2,2)=+fidpoints_[1].x(); M1(2,3)=-fidpoints_[1].y();
00224         M1(3,0)=0.; M1(3,1)=1.; M1(3,2)=+fidpoints_[1].y(); M1(3,3)=+fidpoints_[1].x();
00225         M2(0,0)=1.; M2(0,1)=0.; M2(0,2)=+fidpoints_[2].x(); M2(0,3)=-fidpoints_[2].y();
00226         M2(1,0)=0.; M2(1,1)=1.; M2(1,2)=+fidpoints_[2].y(); M2(1,3)=+fidpoints_[2].x();
00227         M2(2,0)=1.; M2(2,1)=0.; M2(2,2)=+fidpoints_[3].x(); M2(2,3)=-fidpoints_[3].y();
00228         M2(3,0)=0.; M2(3,1)=1.; M2(3,2)=+fidpoints_[3].y(); M2(3,3)=+fidpoints_[3].x();
00229 
00230         //std::cout << "M1:\n" << M1 << std::endl;
00231         //std::cout << "M2:\n" << M2 << std::endl;
00232 
00233         ROOT::Math::SVector<value_t,4> mod_tr1, mod_tr2;
00234         mod_tr1 = M1*mod2;
00235         mod_tr2 = M2*mod1;
00236         //std::cout << "mod_tr1: " << mod_tr1 << std::endl;
00237         //std::cout << "mod_tr2: " << mod_tr2 << std::endl;
00238 
00239         fidpoint_t fidpointvec;
00240         fidpointvec.push_back(coord_t(mod_tr1(0),mod_tr1(1)));
00241         fidpointvec.push_back(coord_t(mod_tr1(2),mod_tr1(3)));
00242         fidpointvec.push_back(coord_t(mod_tr2(0),mod_tr2(1)));
00243         fidpointvec.push_back(coord_t(mod_tr2(2),mod_tr2(3)));
00244 
00245         doFit(fidpointvec);
00246 }
00247 
00248 SurveyPxbImageLocalFit::localpars_t SurveyPxbImageLocalFit::getLocalParameters()
00249 {
00250         if (!fitValidFlag_) throw std::logic_error("SurveyPxbImageLocalFit::getLocalParameters(): Fit is not valid. Call doFit(...) before calling this function.");
00251         return a_;
00252 }
00253 
00254 SurveyPxbImageLocalFit::value_t SurveyPxbImageLocalFit::getChi2()
00255 {
00256         if (!fitValidFlag_) throw std::logic_error("SurveyPxbImageLocalFit::getChi2(): Fit is not valid. Call doFit(...) before calling this function.");
00257         return chi2_;
00258 }
00259 
00260 void SurveyPxbImageLocalFit::setLocalDerivsToZero(count_t j)
00261 {
00262     if (!(j < nLcD)) throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
00263     for(count_t i=0; i!=nMsrmts; i++)
00264         localDerivsMatrix_(i,j)=0;
00265 }
00266 
00267 void SurveyPxbImageLocalFit::setGlobalDerivsToZero(count_t j)
00268 {
00269     if (!(j < nGlD)) throw std::range_error("SurveyPxbImageLocalFit::setLocalDerivsToZero(j): j out of range.");
00270     for(count_t i=0; i!=nMsrmts; i++)
00271         globalDerivsMatrix_(i,j)=0;
00272 }
00273 
00274
/data/refman/pasoursint/CMSSW_5_3_3/src/Alignment/SurveyAnalysis/src/SurveyPxbImageLocalFit.cc
