CommonAlignment/src/SurveyResidual.cc

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   // Find mother matching given type
0014
0015   theMother = &ali;  // start finding from this alignable
0016
0017   while (theMother->alignableObjectId() != type) {
0018     theMother = theMother->mother();  // move up a level
0019
0020     if (!theMother)
0021       return;
0022     //     {
0023     //       throw cms::Exception("ConfigError")
0024     //  << "Alignable (id = " << ali.geomDetId().rawId()
0025     //  << ") does not belong to a composite of type " << type;
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;  // selected parameters
0046
0047   pars.reserve(AlignParams::kSize);
0048
0049   // Find linear displacements.
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   // Match the centers of current and nominal surfaces to find the angular
0061   // displacements about the center. Only do this if angular dof are selected.
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];  // move to nominal pos
0069       currentVs[j] -= theCurrentVs[0];  // move to current pos
0070     }
0071
0072     RotationType rot = diffRot(nominalVs, currentVs);  // frame rotation
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());  // (deltaR, deltaW)
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;  // selected 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     //     if (dau != ali || bias) theSisters.push_back(dau);
0149   }
0150 }
0151
0152 void SurveyResidual::calculate(const Alignable& ali) {
0153   unsigned int nSister = theSisters.size();
0154
0155   // First get sisters' positions
0156
0157   std::vector<const PositionType*> nominalSisPos;  // nominal sisters' pos
0158   std::vector<const PositionType*> currentSisPos;  // current sisters' pos
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   // Then find mother's position using sisters' positions
0177
0178   PositionType nominalMomPos = motherPosition(nominalSisPos);
0179   PositionType currentMomPos = motherPosition(currentSisPos);
0180
0181   // Now find rotation from nominal mother to current mother
0182
0183   GlobalVectors nominalSisVs;  // nominal sisters' pos from mother's pos
0184   GlobalVectors currentSisVs;  // current sisters' pos from mother's pos
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       //       nominalSisVs.push_back(nominalSisPoints[j] - nominalMomPos);
0196       //       currentSisVs.push_back(currentSisPoints[j] - currentMomPos);
0197     }
0198   }
0199
0200   RotationType toCurrent = diffRot(currentSisVs, nominalSisVs);
0201
0202   // Finally shift and rotate nominal sensor to current sensor
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   // Find the covariance
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;  // 6 by 6 Jacobian init to 0
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 // AlgebraicMatrix SurveyResidual::errorTransform(const RotationType& initialFrame,
0254 //                         const RotationType& currentFrame) const
0255 // {
0256 // //   align::EulerAngles angles = align::toAngles(r);
0257
0258 // //   align::Scalar s1 = std::sin(angles[0]), c1 = std::cos(angles[0]);
0259 // //   align::Scalar s2 = std::sin(angles[1]), c2 = std::cos(angles[1]);
0260 // //   align::Scalar s3 = std::sin(angles[2]), c3 = std::cos(angles[2]);
0261
0262 //   AlgebraicMatrix drdw(9, 3, 0); // 9 by 3 Jacobian init to 0
0263
0264 // //   drdw(1, 1) =  0;
0265 // //   drdw(1, 2) =  -s2 * c3;
0266 // //   drdw(1, 3) =  c2 * -s3;
0267 // //   drdw(2, 1) =  -s1 * s3 + c1 * s2 * c3;
0268 // //   drdw(2, 2) =  s1 * c2 * c3;
0269 // //   drdw(2, 3) =  c1 * c3 - s1 * s2 * s3;
0270 // //   drdw(3, 1) =  c1 * s3 + s1 * s2 * c3;
0271 // //   drdw(3, 2) =  -c1 * c2 * c3;
0272 // //   drdw(3, 3) =  s1 * c3 + c1 * s2 * s3;
0273 // //   drdw(4, 1) =  0;
0274 // //   drdw(4, 2) =  s2 * s3;
0275 // //   drdw(4, 3) =  -c2 * c3;
0276 // //   drdw(5, 1) =  -s1 * c3 - c1 * s2 * s3;
0277 // //   drdw(5, 2) =  -s1 * c2 * s3;
0278 // //   drdw(5, 3) =  c1 * -s3 - s1 * s2 * c3;
0279 // //   drdw(6, 1) =  c1 * c3 - s1 * s2 * s3;
0280 // //   drdw(6, 2) =  c1 * c2 * s3;
0281 // //   drdw(6, 3) =  s1 * -s3 + c1 * s2 * c3;
0282 // //   drdw(7, 1) =  0;
0283 // //   drdw(7, 2) =  c2;
0284 // //   drdw(7, 3) =  0;
0285 // //   drdw(8, 1) =  -c1 * c2;
0286 // //   drdw(8, 2) =  s1 * s2;
0287 // //   drdw(8, 3) =  0;
0288 // //   drdw(9, 1) =  -s1 * c2;
0289 // //   drdw(9, 2) =  c1 * -s2;
0290 // //   drdw(9, 3) =  0;
0291 //   drdw(2, 3) = drdw(6, 1) = drdw(7, 2) =  1;
0292 //   drdw(3, 2) = drdw(4, 3) = drdw(8, 1) = -1;
0293
0294 //   align::RotationType deltaR = initialFrame * currentFrame.transposed();
0295
0296 //   AlgebraicMatrix dRdr(9, 9, 0); // 9 by 9 Jacobian init to 0
0297
0298 //   dRdr(1, 1) = deltaR.xx(); dRdr(1, 2) = deltaR.yx(); dRdr(1, 3) = deltaR.zx();
0299 //   dRdr(2, 1) = deltaR.xy(); dRdr(2, 2) = deltaR.yy(); dRdr(2, 3) = deltaR.zy();
0300 //   dRdr(3, 1) = deltaR.xz(); dRdr(3, 2) = deltaR.yz(); dRdr(3, 3) = deltaR.zz();
0301 //   dRdr(4, 4) = deltaR.xx(); dRdr(4, 5) = deltaR.yx(); dRdr(4, 6) = deltaR.zx();
0302 //   dRdr(5, 4) = deltaR.xy(); dRdr(5, 5) = deltaR.yy(); dRdr(5, 6) = deltaR.zy();
0303 //   dRdr(6, 4) = deltaR.xz(); dRdr(6, 5) = deltaR.yz(); dRdr(6, 6) = deltaR.zz();
0304 //   dRdr(7, 7) = deltaR.xx(); dRdr(7, 8) = deltaR.yx(); dRdr(7, 9) = deltaR.zx();
0305 //   dRdr(8, 7) = deltaR.xy(); dRdr(8, 8) = deltaR.yy(); dRdr(8, 9) = deltaR.zy();
0306 //   dRdr(9, 7) = deltaR.xz(); dRdr(9, 8) = deltaR.yz(); dRdr(9, 9) = deltaR.zz();
0307
0308 // //   align::RotationType R = r * deltaR;
0309
0310 //   AlgebraicMatrix dWdR(3, 9, 0); // 3 by 9 Jacobian init to 0
0311
0312 //   align::Scalar R11 = deltaR.xx(), R21 = deltaR.yx();
0313 //   align::Scalar R31 = deltaR.zx(), R32 = deltaR.zy(), R33 = deltaR.zz();
0314
0315 //   align::Scalar den1 = R32 * R32 + R33 * R33;
0316 //   align::Scalar den3 = R11 * R11 + R21 * R21;
0317
0318 //   dWdR(1, 8) = -R33 / den1; dWdR(1, 9) = R32 / den1;
0319 //   dWdR(2, 7) = 1 / std::sqrt(1 - R31 * R31);
0320 //   dWdR(3, 1) = R21 / den3; dWdR(3, 4) = -R11 / den3;
0321
0322 //   AlgebraicMatrix dPdp(6, 6, 0);
0323
0324 //   dPdp(1, 1) = deltaR.xx(); dPdp(1, 2) = deltaR.xy(); dPdp(1, 3) = deltaR.xz();
0325 //   dPdp(2, 1) = deltaR.yx(); dPdp(2, 2) = deltaR.yy(); dPdp(2, 3) = deltaR.yz();
0326 //   dPdp(3, 1) = deltaR.zx(); dPdp(3, 2) = deltaR.zy(); dPdp(3, 3) = deltaR.zz();
0327
0328 //   dPdp.sub(4, 4, dWdR * dRdr * drdw);
0329
0330 //   return dPdp;
0331 // }