CommonAlignmentParametrization/src/RigidBodyAlignmentParameters.cc

0001 /** \file RigidBodyAlignmentParameters.cc
0002  *
0003  *  Version    : $Revision: 1.13 $
0004  *  last update: $Date: 2007/10/08 15:56:01 $
0005  *  by         : $Author: cklae $
0006  */
0007
0008 #include "FWCore/Utilities/interface/Exception.h"
0009
0010 #include "Alignment/CommonAlignment/interface/Alignable.h"
0011 #include "Alignment/CommonAlignment/interface/AlignableDetOrUnitPtr.h"
0012 #include "Alignment/CommonAlignmentParametrization/interface/AlignmentParametersFactory.h"
0013 #include "Alignment/CommonAlignmentParametrization/interface/FrameToFrameDerivative.h"
0014 #include "Alignment/CommonAlignmentParametrization/interface/KarimakiAlignmentDerivatives.h"
0015 #include "CondFormats/Alignment/interface/Definitions.h"
0016
0017 // This class's header
0018 #include "Alignment/CommonAlignmentParametrization/interface/RigidBodyAlignmentParameters.h"
0019
0020 //__________________________________________________________________________________________________
0021 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable *ali, bool calcMis)
0022     : AlignmentParameters(ali, displacementFromAlignable(calcMis ? ali : nullptr), AlgebraicSymMatrix(N_PARAM, 0)) {}
0023
0024 //__________________________________________________________________________________________________
0025 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable *alignable,
0026                                                            const AlgebraicVector &parameters,
0027                                                            const AlgebraicSymMatrix &covMatrix)
0028     : AlignmentParameters(alignable, parameters, covMatrix) {
0029   if (parameters.num_row() != N_PARAM) {
0030     throw cms::Exception("BadParameters")
0031         << "in RigidBodyAlignmentParameters(): " << parameters.num_row() << " instead of " << N_PARAM << " parameters.";
0032   }
0033 }
0034
0035 //__________________________________________________________________________________________________
0036 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable *alignable,
0037                                                            const AlgebraicVector &parameters,
0038                                                            const AlgebraicSymMatrix &covMatrix,
0039                                                            const std::vector<bool> &selection)
0040     : AlignmentParameters(alignable, parameters, covMatrix, selection) {
0041   if (parameters.num_row() != N_PARAM) {
0042     throw cms::Exception("BadParameters")
0043         << "in RigidBodyAlignmentParameters(): " << parameters.num_row() << " instead of " << N_PARAM << " parameters.";
0044   }
0045 }
0046
0047 //__________________________________________________________________________________________________
0048 RigidBodyAlignmentParameters *RigidBodyAlignmentParameters::clone(const AlgebraicVector &parameters,
0049                                                                   const AlgebraicSymMatrix &covMatrix) const {
0050   RigidBodyAlignmentParameters *rbap = new RigidBodyAlignmentParameters(alignable(), parameters, covMatrix, selector());
0051
0052   if (userVariables())
0053     rbap->setUserVariables(userVariables()->clone());
0054   rbap->setValid(isValid());
0055
0056   return rbap;
0057 }
0058
0059 //__________________________________________________________________________________________________
0060 RigidBodyAlignmentParameters *RigidBodyAlignmentParameters::cloneFromSelected(
0061     const AlgebraicVector &parameters, const AlgebraicSymMatrix &covMatrix) const {
0062   RigidBodyAlignmentParameters *rbap = new RigidBodyAlignmentParameters(
0063       alignable(), expandVector(parameters, selector()), expandSymMatrix(covMatrix, selector()), selector());
0064
0065   if (userVariables())
0066     rbap->setUserVariables(userVariables()->clone());
0067   rbap->setValid(isValid());
0068
0069   return rbap;
0070 }
0071
0072 //__________________________________________________________________________________________________
0073 AlgebraicMatrix RigidBodyAlignmentParameters::derivatives(const TrajectoryStateOnSurface &tsos,
0074                                                           const AlignableDetOrUnitPtr &alidet) const {
0075   const Alignable *ali = this->alignable();  // Alignable of these parameters
0076
0077   if (ali == alidet) {  // same alignable => same frame
0078     return KarimakiAlignmentDerivatives()(tsos);
0079   } else {  // different alignable => transform into correct frame
0080     const AlgebraicMatrix deriv = KarimakiAlignmentDerivatives()(tsos);
0081     FrameToFrameDerivative ftfd;
0082     return ftfd.frameToFrameDerivative(alidet, ali).T() * deriv;
0083   }
0084 }
0085
0086 //__________________________________________________________________________________________________
0087 AlgebraicMatrix RigidBodyAlignmentParameters::selectedDerivatives(const TrajectoryStateOnSurface &tsos,
0088                                                                   const AlignableDetOrUnitPtr &alignableDet) const {
0089   const AlgebraicMatrix dev = this->derivatives(tsos, alignableDet);
0090
0091   int ncols = dev.num_col();
0092   int nrows = dev.num_row();
0093   int nsel = numSelected();
0094
0095   AlgebraicMatrix seldev(nsel, ncols);
0096
0097   int ir2 = 0;
0098   for (int irow = 0; irow < nrows; ++irow) {
0099     if (selector()[irow]) {
0100       for (int icol = 0; icol < ncols; ++icol)
0101         seldev[ir2][icol] = dev[irow][icol];
0102       ++ir2;
0103     }
0104   }
0105
0106   return seldev;
0107 }
0108
0109 //__________________________________________________________________________________________________
0110 AlgebraicVector RigidBodyAlignmentParameters::translation(void) const {
0111   AlgebraicVector shift(3);
0112   for (int i = 0; i < 3; ++i)
0113     shift[i] = theData->parameters()[i];
0114
0115   return shift;
0116 }
0117
0118 //__________________________________________________________________________________________________
0119 AlgebraicVector RigidBodyAlignmentParameters::rotation(void) const {
0120   AlgebraicVector rot(3);
0121   for (int i = 0; i < 3; ++i)
0122     rot[i] = theData->parameters()[i + 3];
0123
0124   return rot;
0125 }
0126
0127 //__________________________________________________________________________________________________
0128 void RigidBodyAlignmentParameters::apply() {
0129   Alignable *alignable = this->alignable();
0130   if (!alignable) {
0131     throw cms::Exception("BadParameters") << "RigidBodyAlignmentParameters::apply: parameters without alignable";
0132   }
0133
0134   // Translation in local frame
0135   AlgebraicVector shift = this->translation();  // fixme: should be LocalVector
0136
0137   // Translation local->global
0138   align::LocalVector lv(shift[0], shift[1], shift[2]);
0139   alignable->move(alignable->surface().toGlobal(lv));
0140
0141   // Rotation in local frame
0142   align::EulerAngles angles = this->rotation();
0143   // original code:
0144   //  alignable->rotateInLocalFrame( align::toMatrix(angles) );
0145   // correct for rounding errors:
0146   align::RotationType rot = alignable->surface().toGlobal(align::toMatrix(angles));
0147   align::rectify(rot);
0148   alignable->rotateInGlobalFrame(rot);
0149 }
0150
0151 //__________________________________________________________________________________________________
0152 int RigidBodyAlignmentParameters::type() const { return AlignmentParametersFactory::kRigidBody; }
0153
0154 //__________________________________________________________________________________________________
0155 AlgebraicVector RigidBodyAlignmentParameters::globalParameters(void) const {
0156   AlgebraicVector m_GlobalParameters(N_PARAM, 0);
0157
0158   const AlgebraicVector shift = translation();  // fixme: should return LocalVector
0159
0160   const align::LocalVector lv(shift[0], shift[1], shift[2]);
0161   const align::GlobalVector dg = theAlignable->surface().toGlobal(lv);
0162
0163   m_GlobalParameters[0] = dg.x();
0164   m_GlobalParameters[1] = dg.y();
0165   m_GlobalParameters[2] = dg.z();
0166
0167   const align::EulerAngles eulerglob = theAlignable->surface().toGlobal(rotation());
0168
0169   m_GlobalParameters[3] = eulerglob(1);
0170   m_GlobalParameters[4] = eulerglob(2);
0171   m_GlobalParameters[5] = eulerglob(3);
0172
0173   return m_GlobalParameters;
0174 }
0175
0176 //__________________________________________________________________________________________________
0177 void RigidBodyAlignmentParameters::print(void) const {
0178   std::cout << "Contents of RigidBodyAlignmentParameters:"
0179             << "\nParameters: " << theData->parameters() << "\nCovariance: " << theData->covariance() << std::endl;
0180 }
0181
0182 //__________________________________________________________________________________________________
0183 AlgebraicVector RigidBodyAlignmentParameters::displacementFromAlignable(const Alignable *ali) {
0184   AlgebraicVector displacement(N_PARAM);
0185
0186   if (ali) {
0187     const align::RotationType &dR = ali->rotation();
0188
0189     const align::LocalVector shifts(ali->globalRotation() * (dR.transposed() * ali->displacement().basicVector()));
0190
0191     const align::EulerAngles angles = align::toAngles(ali->surface().toLocal(dR));
0192
0193     displacement[0] = shifts.x();
0194     displacement[1] = shifts.y();
0195     displacement[2] = shifts.z();
0196     displacement[3] = angles(1);
0197     displacement[4] = angles(2);
0198     displacement[5] = angles(3);
0199   }
0200
0201   return displacement;
0202 }