Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 12:44:36

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 // }