File indexing completed on 2024-04-06 11:56:07
0001 #include "Alignment/CommonAlignment/interface/Alignable.h"
0002 #include "Alignment/CommonAlignment/interface/AlignmentParameters.h"
0003 #include "Alignment/CommonAlignment/interface/SurveyDet.h"
0004 #include "DataFormats/Math/interface/Matrix.h"
0005 #include "FWCore/Utilities/interface/Exception.h"
0006
0007 #include "Alignment/CommonAlignment/interface/SurveyResidual.h"
0008
0009 using namespace align;
0010
0011 SurveyResidual::SurveyResidual(const Alignable& ali, StructureType type, bool bias)
0012 : theMother(nullptr), theSurface(ali.surface()), theSelector(ali.alignmentParameters()->selector()) {
0013
0014
0015 theMother = &ali;
0016
0017 while (theMother->alignableObjectId() != type) {
0018 theMother = theMother->mother();
0019
0020 if (!theMother)
0021 return;
0022
0023
0024
0025
0026
0027 }
0028
0029 if (!theMother->mother()) {
0030 throw cms::Exception("ConfigError") << "The type " << type << " does not have a survey residual defined!\n"
0031 << "You have probably set the highest hierarchy. Choose a lower level.";
0032 }
0033
0034 findSisters(theMother, bias);
0035
0036 if (theSisters.empty()) {
0037 throw cms::Exception("ConfigError") << "You are finding an unbiased residual of an alignable "
0038 << " (id = " << ali.geomDetId().rawId() << ") which has no sister. Abort!";
0039 }
0040
0041 calculate(ali);
0042 }
0043
0044 AlgebraicVector SurveyResidual::sensorResidual() const {
0045 std::vector<Scalar> pars;
0046
0047 pars.reserve(AlignParams::kSize);
0048
0049
0050
0051 align::LocalVector deltaR = theSurface.toLocal(theCurrentVs[0] - theNominalVs[0]);
0052
0053 if (theSelector[0])
0054 pars.push_back(deltaR.x());
0055 if (theSelector[1])
0056 pars.push_back(deltaR.y());
0057 if (theSelector[2])
0058 pars.push_back(deltaR.z());
0059
0060
0061
0062
0063 if (theSelector[3] || theSelector[4] || theSelector[5]) {
0064 GlobalVectors nominalVs = theNominalVs;
0065 GlobalVectors currentVs = theCurrentVs;
0066
0067 for (unsigned int j = 0; j < nominalVs.size(); ++j) {
0068 nominalVs[j] -= theNominalVs[0];
0069 currentVs[j] -= theCurrentVs[0];
0070 }
0071
0072 RotationType rot = diffRot(nominalVs, currentVs);
0073
0074 EulerAngles deltaW = toAngles(theSurface.toLocal(rot));
0075
0076 if (theSelector[3])
0077 pars.push_back(deltaW(1));
0078 if (theSelector[4])
0079 pars.push_back(deltaW(2));
0080 if (theSelector[5])
0081 pars.push_back(deltaW(3));
0082 }
0083
0084 AlgebraicVector deltaRW(pars.size());
0085
0086 for (unsigned int j = 0; j < pars.size(); ++j)
0087 deltaRW(j + 1) = pars[j];
0088
0089 return deltaRW;
0090 }
0091
0092 LocalVectors SurveyResidual::pointsResidual() const {
0093 LocalVectors residuals;
0094
0095 unsigned int nPoint = theNominalVs.size();
0096
0097 residuals.reserve(nPoint);
0098
0099 for (unsigned int j = 0; j < nPoint; ++j) {
0100 residuals.push_back(theSurface.toLocal(theCurrentVs[j] - theNominalVs[j]));
0101 }
0102
0103 return residuals;
0104 }
0105
0106 AlgebraicSymMatrix SurveyResidual::inverseCovariance() const {
0107 if (theSelector.size() != ErrorMatrix::kRows) {
0108 throw cms::Exception("LogicError") << "Mismatched number of dof between ErrorMatrix and Selector.";
0109 }
0110
0111 std::vector<unsigned int> indices;
0112
0113 indices.reserve(ErrorMatrix::kRows);
0114
0115 for (unsigned int i = 0; i < ErrorMatrix::kRows; ++i)
0116 if (theSelector[i])
0117 indices.push_back(i);
0118
0119 AlgebraicSymMatrix invCov(indices.size());
0120
0121 for (unsigned int i = 0; i < indices.size(); ++i)
0122 for (unsigned int j = 0; j <= i; ++j)
0123 invCov.fast(i + 1, j + 1) = theCovariance(indices[i], indices[j]);
0124
0125 int fail(0);
0126 invCov.invert(fail);
0127
0128 if (fail) {
0129 throw cms::Exception("ConfigError") << "Cannot invert survey error " << invCov;
0130 }
0131
0132 return invCov;
0133 }
0134
0135 void SurveyResidual::findSisters(const Alignable* ali, bool bias) {
0136 theSisters.clear();
0137 theSisters.reserve(1000);
0138
0139 const auto& comp = ali->mother()->components();
0140
0141 unsigned int nComp = comp.size();
0142
0143 for (unsigned int i = 0; i < nComp; ++i) {
0144 const Alignable* dau = comp[i];
0145
0146 if (dau != ali || bias)
0147 theSisters.insert(theSisters.end(), dau->deepComponents().begin(), dau->deepComponents().end());
0148
0149 }
0150 }
0151
0152 void SurveyResidual::calculate(const Alignable& ali) {
0153 unsigned int nSister = theSisters.size();
0154
0155
0156
0157 std::vector<const PositionType*> nominalSisPos;
0158 std::vector<const PositionType*> currentSisPos;
0159
0160 nominalSisPos.reserve(nSister);
0161 currentSisPos.reserve(nSister);
0162
0163 for (unsigned int i = 0; i < nSister; ++i) {
0164 const Alignable* sis = theSisters[i];
0165 const SurveyDet* survey = sis->survey();
0166
0167 if (!survey) {
0168 throw cms::Exception("ConfigError") << "No survey info is found for Alignable "
0169 << " (id = " << sis->geomDetId().rawId() << "). Abort!";
0170 }
0171
0172 nominalSisPos.push_back(&survey->position());
0173 currentSisPos.push_back(&sis->globalPosition());
0174 }
0175
0176
0177
0178 PositionType nominalMomPos = motherPosition(nominalSisPos);
0179 PositionType currentMomPos = motherPosition(currentSisPos);
0180
0181
0182
0183 GlobalVectors nominalSisVs;
0184 GlobalVectors currentSisVs;
0185
0186 for (unsigned int i = 0; i < nSister; ++i) {
0187 const Alignable* sis = theSisters[i];
0188
0189 const GlobalPoints& nominalSisPoints = sis->survey()->globalPoints();
0190 const GlobalPoints& currentSisPoints = sis->surface().toGlobal(sis->survey()->localPoints());
0191
0192 for (unsigned int j = 0; j < nominalSisPoints.size(); ++j) {
0193 nominalSisVs.push_back(nominalSisPoints[j] - *nominalSisPos[i]);
0194 currentSisVs.push_back(currentSisPoints[j] - *currentSisPos[i]);
0195
0196
0197 }
0198 }
0199
0200 RotationType toCurrent = diffRot(currentSisVs, nominalSisVs);
0201
0202
0203
0204 const SurveyDet* survey = ali.survey();
0205
0206 if (!survey) {
0207 throw cms::Exception("ConfigError") << "No survey info is found for Alignable "
0208 << " (id = " << ali.geomDetId().rawId() << "). Abort!";
0209 }
0210
0211 const GlobalPoints& nominalPoints = survey->globalPoints();
0212 const GlobalPoints& currentPoints = theSurface.toGlobal(survey->localPoints());
0213
0214 for (unsigned int j = 0; j < nominalPoints.size(); ++j) {
0215 align::GlobalVector nv = nominalPoints[j] - nominalMomPos;
0216
0217 theNominalVs.push_back(align::GlobalVector(toCurrent * nv.basicVector()));
0218 theCurrentVs.push_back(currentPoints[j] - currentMomPos);
0219 }
0220
0221
0222
0223 const RotationType& currentFrame = ali.globalRotation();
0224
0225 for (const Alignable* a = &ali; a != theMother->mother(); a = a->mother()) {
0226 RotationType deltaR = currentFrame * a->survey()->rotation().transposed();
0227
0228 math::Matrix<6, 6>::type jac;
0229
0230 jac(0, 0) = deltaR.xx();
0231 jac(0, 1) = deltaR.xy();
0232 jac(0, 2) = deltaR.xz();
0233 jac(1, 0) = deltaR.yx();
0234 jac(1, 1) = deltaR.yy();
0235 jac(1, 2) = deltaR.yz();
0236 jac(2, 0) = deltaR.zx();
0237 jac(2, 1) = deltaR.zy();
0238 jac(2, 2) = deltaR.zz();
0239 jac(3, 3) = deltaR.xx();
0240 jac(3, 4) = deltaR.xy();
0241 jac(3, 5) = deltaR.xz();
0242 jac(4, 3) = deltaR.yx();
0243 jac(4, 4) = deltaR.yy();
0244 jac(4, 5) = deltaR.yz();
0245 jac(5, 3) = deltaR.zx();
0246 jac(5, 4) = deltaR.zy();
0247 jac(5, 5) = deltaR.zz();
0248
0249 theCovariance += ROOT::Math::Similarity(jac, a->survey()->errors());
0250 }
0251 }
0252
0253
0254
0255
0256
0257
0258
0259
0260
0261
0262
0263
0264
0265
0266
0267
0268
0269
0270
0271
0272
0273
0274
0275
0276
0277
0278
0279
0280
0281
0282
0283
0284
0285
0286
0287
0288
0289
0290
0291
0292
0293
0294
0295
0296
0297
0298
0299
0300
0301
0302
0303
0304
0305
0306
0307
0308
0309
0310
0311
0312
0313
0314
0315
0316
0317
0318
0319
0320
0321
0322
0323
0324
0325
0326
0327
0328
0329
0330
0331