CMS 3D CMS Logo

/data/refman/pasoursint/CMSSW_5_3_3/src/Alignment/CommonAlignmentParametrization/src/RigidBodyAlignmentParameters.cc

Go to the documentation of this file.
00001 
00008 #include "FWCore/Utilities/interface/Exception.h"
00009 
00010 #include "Alignment/CommonAlignment/interface/Alignable.h"
00011 #include "Alignment/CommonAlignment/interface/AlignableDetOrUnitPtr.h"
00012 #include "Alignment/CommonAlignmentParametrization/interface/FrameToFrameDerivative.h"
00013 #include "Alignment/CommonAlignmentParametrization/interface/KarimakiAlignmentDerivatives.h"
00014 #include "Alignment/CommonAlignmentParametrization/interface/AlignmentParametersFactory.h"
00015 #include "CondFormats/Alignment/interface/Definitions.h"
00016 
00017 // This class's header 
00018 #include "Alignment/CommonAlignmentParametrization/interface/RigidBodyAlignmentParameters.h"
00019 
00020 //__________________________________________________________________________________________________
00021 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable* ali, bool calcMis) :
00022   AlignmentParameters(ali, displacementFromAlignable(calcMis ? ali : 0),
00023                       AlgebraicSymMatrix(N_PARAM, 0))
00024 {
00025 }
00026 
00027 //__________________________________________________________________________________________________
00028 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable* alignable, 
00029                                                            const AlgebraicVector& parameters, 
00030                                                            const AlgebraicSymMatrix& covMatrix) :
00031   AlignmentParameters( alignable, parameters, covMatrix )
00032 {
00033  
00034   if (parameters.num_row() != N_PARAM) {
00035     throw cms::Exception("BadParameters") << "in RigidBodyAlignmentParameters(): "
00036                                           << parameters.num_row() << " instead of " << N_PARAM 
00037                                           << " parameters.";
00038   }
00039 }
00040 
00041 //__________________________________________________________________________________________________
00042 RigidBodyAlignmentParameters::RigidBodyAlignmentParameters(Alignable* alignable, 
00043                                                            const AlgebraicVector& parameters, 
00044                                                            const AlgebraicSymMatrix& covMatrix,
00045                                                            const std::vector<bool>& selection ) :
00046   AlignmentParameters( alignable, parameters, covMatrix, selection )
00047 {  
00048   if (parameters.num_row() != N_PARAM) {
00049     throw cms::Exception("BadParameters") << "in RigidBodyAlignmentParameters(): "
00050                                           << parameters.num_row() << " instead of " << N_PARAM 
00051                                           << " parameters.";
00052   }
00053 }
00054 
00055 //__________________________________________________________________________________________________
00056 RigidBodyAlignmentParameters* 
00057 RigidBodyAlignmentParameters::clone( const AlgebraicVector& parameters, 
00058                                      const AlgebraicSymMatrix& covMatrix ) const 
00059 {
00060   RigidBodyAlignmentParameters* rbap = 
00061     new RigidBodyAlignmentParameters( alignable(), parameters, covMatrix, selector());
00062 
00063   if (userVariables()) rbap->setUserVariables(userVariables()->clone());
00064   rbap->setValid(isValid());
00065 
00066   return rbap;
00067 }
00068 
00069 //__________________________________________________________________________________________________
00070 RigidBodyAlignmentParameters* 
00071 RigidBodyAlignmentParameters::cloneFromSelected( const AlgebraicVector& parameters,
00072                                                  const AlgebraicSymMatrix& covMatrix ) const
00073 {
00074   RigidBodyAlignmentParameters* rbap = 
00075     new RigidBodyAlignmentParameters(alignable(), expandVector( parameters, selector()),
00076                                      expandSymMatrix(covMatrix, selector()), selector());
00077 
00078   if ( userVariables() ) rbap->setUserVariables(userVariables()->clone());
00079   rbap->setValid(isValid());
00080 
00081   return rbap;
00082 }
00083 
00084 //__________________________________________________________________________________________________
00085 AlgebraicMatrix 
00086 RigidBodyAlignmentParameters::derivatives( const TrajectoryStateOnSurface &tsos,
00087                                            const AlignableDetOrUnitPtr &alidet ) const
00088 {
00089   const Alignable *ali = this->alignable(); // Alignable of these parameters
00090 
00091   if (ali == alidet) { // same alignable => same frame
00092     return KarimakiAlignmentDerivatives()(tsos);
00093   } else { // different alignable => transform into correct frame
00094     const AlgebraicMatrix deriv = KarimakiAlignmentDerivatives()(tsos);
00095     FrameToFrameDerivative ftfd;
00096     return ftfd.frameToFrameDerivative(alidet, ali) * deriv;
00097   }
00098 }
00099 
00100 
00101 //__________________________________________________________________________________________________
00102 AlgebraicMatrix 
00103 RigidBodyAlignmentParameters::selectedDerivatives( const TrajectoryStateOnSurface& tsos, 
00104                                                    const AlignableDetOrUnitPtr &alignableDet ) const
00105 {
00106   const AlgebraicMatrix dev = this->derivatives( tsos, alignableDet );
00107 
00108   int ncols  = dev.num_col();
00109   int nrows  = dev.num_row();
00110   int nsel   = numSelected();
00111 
00112   AlgebraicMatrix seldev( nsel, ncols );
00113 
00114   int ir2=0;
00115   for ( int irow=0; irow<nrows; ++irow ) {
00116     if (selector()[irow]) {
00117       for ( int icol=0; icol<ncols; ++icol ) seldev[ir2][icol] = dev[irow][icol];
00118       ++ir2;
00119     }
00120   }
00121 
00122   return seldev;
00123 }
00124 
00125 
00126 //__________________________________________________________________________________________________
00127 AlgebraicVector RigidBodyAlignmentParameters::translation(void) const
00128 { 
00129   AlgebraicVector shift(3);
00130   for ( int i=0;i<3;++i ) shift[i]=theData->parameters()[i];
00131 
00132   return shift;
00133 }
00134 
00135 
00136 //__________________________________________________________________________________________________
00137 AlgebraicVector RigidBodyAlignmentParameters::rotation(void) const
00138 {
00139   AlgebraicVector rot(3);
00140   for (int i=0;i<3;++i) rot[i] = theData->parameters()[i+3];
00141 
00142   return rot;
00143 }
00144 
00145 //__________________________________________________________________________________________________
00146 void RigidBodyAlignmentParameters::apply()
00147 {
00148   Alignable *alignable = this->alignable();
00149   if (!alignable) {
00150     throw cms::Exception("BadParameters") 
00151       << "RigidBodyAlignmentParameters::apply: parameters without alignable";
00152   }
00153   
00154   // Translation in local frame
00155   AlgebraicVector shift = this->translation(); // fixme: should be LocalVector
00156 
00157   // Translation local->global
00158   align::LocalVector lv(shift[0], shift[1], shift[2]);
00159   alignable->move( alignable->surface().toGlobal(lv) );
00160 
00161   // Rotation in local frame
00162   align::EulerAngles angles = this->rotation();
00163   // original code:
00164   //  alignable->rotateInLocalFrame( align::toMatrix(angles) );
00165   // correct for rounding errors:
00166   align::RotationType rot = alignable->surface().toGlobal( align::toMatrix(angles) );
00167   align::rectify(rot);
00168   alignable->rotateInGlobalFrame(rot);
00169 }
00170 
00171 //__________________________________________________________________________________________________
00172 int RigidBodyAlignmentParameters::type() const
00173 {
00174   return AlignmentParametersFactory::kRigidBody;
00175 }
00176 
00177 //__________________________________________________________________________________________________
00178 AlgebraicVector RigidBodyAlignmentParameters::globalParameters(void) const
00179 {
00180   AlgebraicVector m_GlobalParameters(N_PARAM, 0);
00181 
00182   const AlgebraicVector shift = translation(); // fixme: should return LocalVector
00183 
00184   const align::LocalVector lv(shift[0], shift[1], shift[2]);
00185   const align::GlobalVector dg = theAlignable->surface().toGlobal(lv);
00186 
00187   m_GlobalParameters[0] = dg.x();
00188   m_GlobalParameters[1] = dg.y();
00189   m_GlobalParameters[2] = dg.z();
00190 
00191   const align::EulerAngles eulerglob = theAlignable->surface().toGlobal( rotation() );
00192 
00193   m_GlobalParameters[3]=eulerglob(1);
00194   m_GlobalParameters[4]=eulerglob(2);
00195   m_GlobalParameters[5]=eulerglob(3);
00196 
00197   return m_GlobalParameters;
00198 }
00199 
00200 
00201 //__________________________________________________________________________________________________
00202 void RigidBodyAlignmentParameters::print(void) const
00203 {
00204 
00205   std::cout << "Contents of RigidBodyAlignmentParameters:"
00206             << "\nParameters: " << theData->parameters()
00207             << "\nCovariance: " << theData->covariance() << std::endl;
00208 }
00209 
00210 
00211 //__________________________________________________________________________________________________
00212 AlgebraicVector RigidBodyAlignmentParameters::displacementFromAlignable(const Alignable* ali)
00213 {
00214   AlgebraicVector displacement(N_PARAM);
00215 
00216   if (ali) {
00217     const align::RotationType& dR = ali->rotation();
00218     
00219     const align::LocalVector shifts( ali->globalRotation() * 
00220                                      ( dR.transposed() * ali->displacement().basicVector() ) );
00221 
00222     const align::EulerAngles angles = align::toAngles( ali->surface().toLocal(dR) );
00223 
00224     displacement[0] = shifts.x();
00225     displacement[1] = shifts.y();
00226     displacement[2] = shifts.z();
00227     displacement[3] = angles(1);
00228     displacement[4] = angles(2);
00229     displacement[5] = angles(3);
00230   }
00231 
00232   return displacement;
00233 }