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
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
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
0057 ROOT::Math::SMatrix<value_t, nMsrmts, nLcD> A;
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
0095 ROOT::Math::SMatrix<value_t, nMsrmts, nMsrmts> W;
0096
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
0112 ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWA;
0113 ATWA = ROOT::Math::Transpose(A) * W * A;
0114
0115
0116 ROOT::Math::SMatrix<value_t, nLcD, nLcD> ATWAi;
0117 int ifail = 0;
0118 ATWAi = ATWA.Inverse(ifail);
0119 if (ifail != 0) {
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
0129 ROOT::Math::SVector<value_t, nMsrmts> y;
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
0143 ROOT::Math::SVector<value_t, nLcD> a;
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
0154 r = y - A * a;
0155 #ifdef DEBUG
0156 std::cout << "r: " << r << std::endl;
0157 #endif
0158
0159
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
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
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
0243
0244
0245
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
0281
0282
0283 ROOT::Math::SVector<value_t, 4> mod_tr1, mod_tr2;
0284 mod_tr1 = M1 * mod2;
0285 mod_tr2 = M2 * mod1;
0286
0287
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 }