AnalyticalJacobians/test/ErrorPropOri_t.cpp

0001 #include "TrackingTools/GeomPropagators/interface/HelixForwardPlaneCrossing.h"
0002 #include "TrackingTools/GeomPropagators/interface/HelixArbitraryPlaneCrossing.h"
0003 #include "TrackingTools/AnalyticalJacobians/interface/AnalyticalCurvilinearJacobian.h"
0004 #include "TrackingTools/AnalyticalJacobians/interface/JacobianLocalToCurvilinear.h"
0005 #include "TrackingTools/AnalyticalJacobians/interface/JacobianCurvilinearToLocal.h"
0006 #include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
0007
0008 #include "DataFormats/GeometrySurface/interface/Surface.h"
0009 #include "TrackingTools/TrajectoryParametrization/interface/LocalTrajectoryParameters.h"
0010
0011 #include "DataFormats/GeometrySurface/interface/Plane.h"
0012
0013 #include "MagneticField/Engine/interface/MagneticField.h"
0014
0015 namespace {
0016
0017   struct M5T : public MagneticField {
0018     M5T() : m(0., 0., 5.) {}
0019     virtual GlobalVector inTesla(const GlobalPoint&) const { return m; }
0020
0021     GlobalVector m;
0022   };
0023
0024 }  // namespace
0025
0026 #include "FWCore/Utilities/interface/HRRealTime.h"
0027 void st() {}
0028 void en() {}
0029
0030 #include <iostream>
0031
0032 int main() {
0033   M5T const m;
0034
0035   Basic3DVector<float> axis(0.5, 1., 1);
0036
0037   Surface::RotationType rot(axis, 0.5 * M_PI);
0038   std::cout << rot << std::endl;
0039
0040   Surface::PositionType pos(0., 0., 0.);
0041
0042   Plane plane(pos, rot);
0043   LocalTrajectoryParameters tpl(-1. / 3.5, 1., 1., 0., 0., 1.);
0044   GlobalVector mg = plane.toGlobal(tpl.momentum());
0045   GlobalTrajectoryParameters tpg(pos, mg, -1., &m);
0046   std::cout << tpl.position() << " " << tpl.momentum() << std::endl;
0047   std::cout << tpg.position() << " " << tpg.momentum() << std::endl;
0048
0049   double curv = tpg.transverseCurvature();
0050
0051   std::cout << curv << " " << mg.mag() << std::endl;
0052
0053   AlgebraicMatrix55 fullJacobian = AlgebraicMatrixID();
0054   AlgebraicMatrix55 deltaJacobian = AlgebraicMatrixID();
0055   GlobalTrajectoryParameters tpg0(pos, mg, -1., &m);
0056   double totalStep(0.);
0057   double singleStep(1.);
0058   for (int i = 0; i < 10; ++i) {
0059     HelixForwardPlaneCrossing prop(HelixForwardPlaneCrossing::PositionType(tpg.position()),
0060                                    HelixForwardPlaneCrossing::DirectionType(tpg.momentum()),
0061                                    curv);
0062     GlobalVector h = tpg.magneticFieldInInverseGeV(tpg.position());
0063     double s = singleStep;
0064     totalStep += singleStep;
0065     GlobalPoint x(prop.position(s));
0066     GlobalVector p(prop.direction(s));
0067     GlobalTrajectoryParameters tpg2(x, p.unit(), curv, 0, &m);
0068     std::cout << x << " " << p << std::endl;
0069     std::cout << tpg2.position() << " " << tpg2.momentum() << std::endl;
0070     AnalyticalCurvilinearJacobian full;
0071     AnalyticalCurvilinearJacobian delta;
0072     full.computeFullJacobian(tpg, tpg2.position(), tpg2.momentum(), h, s);
0073     std::cout << full.jacobian() << std::endl;
0074     std::cout << std::endl;
0075     delta.computeInfinitesimalJacobian(tpg, x, tpg2.momentum(), h, s);
0076     std::cout << delta.jacobian() << std::endl;
0077     std::cout << std::endl;
0078     tpg = tpg2;
0079     fullJacobian *= full.jacobian();
0080     deltaJacobian *= delta.jacobian();
0081   }
0082   std::cout << "---------------------------" << std::endl;
0083   std::cout << std::endl;
0084   std::cout << fullJacobian << std::endl;
0085   std::cout << std::endl;
0086   std::cout << deltaJacobian << std::endl;
0087   std::cout << std::endl;
0088   AnalyticalCurvilinearJacobian full;
0089   GlobalVector h = tpg0.magneticFieldInInverseGeV(tpg0.position());
0090   full.computeFullJacobian(tpg0, tpg.position(), tpg.momentum(), h, totalStep);
0091   std::cout << full.jacobian() << std::endl;
0092   std::cout << std::endl;
0093
0094   AlgebraicMatrix55 div;
0095   for (unsigned int i = 0; i < 5; ++i) {
0096     for (unsigned int j = 0; j < 5; ++j) {
0097       //       div(i,j) = fabs(full.jacobian()(i,j))>1.e-20 ? fullJacobian(i,j)/full.jacobian()(i,j)-1 : 0;
0098       div(i, j) = fullJacobian(i, j) - full.jacobian()(i, j);
0099     }
0100   }
0101   std::cout << "Full relative" << std::endl << div << std::endl;
0102   for (unsigned int i = 0; i < 5; ++i) {
0103     for (unsigned int j = 0; j < 5; ++j) {
0104       //       div(i,j) = fabs(full.jacobian()(i,j))>1.e-20 ? deltaJacobian(i,j)/full.jacobian()(i,j)-1 : 0;
0105       div(i, j) = deltaJacobian(i, j) - full.jacobian()(i, j);
0106     }
0107   }
0108   std::cout << "Delta relative" << std::endl << div << std::endl;
0109
0110   //
0111   // plane at end of propagation (equivalent to Curvilinear definition up to rotation in the plane)
0112   //
0113   Basic3DVector<float> newaxis(tpg.momentum().unit());
0114   Surface::RotationType newrot(axis, 0.);
0115   Surface::PositionType newpos(tpg.position());
0116   Plane newplane(newpos, newrot);
0117   //
0118   // total jacobian (local to local)
0119   //
0120   JacobianLocalToCurvilinear jlc(plane, tpl, m);
0121   LocalTrajectoryParameters newlpg(newplane.toLocal(tpg.position()), newplane.toLocal(tpg.momentum()), 1.);
0122   JacobianCurvilinearToLocal jcl(newplane, newlpg, m);
0123   //   AlgebraicMatrix55 jacobianL2L = jcl.jacobian()*full.jacobian()*jlc.jacobian();
0124   AlgebraicMatrix55 jacobianL2L = jcl.jacobian() * deltaJacobian * jlc.jacobian();
0125   //
0126   // redo propagation to target plane (should give identical position ...)
0127   //
0128   HelixArbitraryPlaneCrossing aprop(HelixForwardPlaneCrossing::PositionType(tpg0.position()),
0129                                     HelixForwardPlaneCrossing::DirectionType(tpg0.momentum()),
0130                                     curv);
0131   std::pair<bool, double> as = aprop.pathLength(newplane);
0132   std::cout << "as = " << as.second << std::endl;
0133   GlobalPoint ax(aprop.position(as.second));
0134   GlobalVector ap(aprop.direction(as.second) * tpg0.momentum().perp());
0135   //
0136   // change initial q/p and redo propagation
0137   //
0138   double qpmod = 1. / tpg0.momentum().mag() + 0.01;
0139   GlobalTrajectoryParameters tpg0mod(tpg0.position(), tpg0.momentum().unit() / fabs(qpmod), qpmod > 0 ? 1. : -1., &m);
0140   HelixArbitraryPlaneCrossing aprop2(HelixForwardPlaneCrossing::PositionType(tpg0mod.position()),
0141                                      HelixForwardPlaneCrossing::DirectionType(tpg0mod.momentum()),
0142                                      tpg0mod.transverseCurvature());
0143   std::pair<bool, double> as2 = aprop2.pathLength(newplane);
0144   std::cout << "as2 = " << as2.second << std::endl;
0145   GlobalPoint ax2(aprop2.position(as2.second));
0146   GlobalVector ap2(aprop2.direction(as2.second) * tpg0mod.momentum().perp());
0147   //
0148   // local coordinates after 2nd propagation
0149   LocalVector newDx = newplane.toLocal(ax2 - ax);
0150   LocalVector newDp = newplane.toLocal(ap2 / ap2.z() - ap / ap.z());
0151   std::cout << newDx << std::endl;
0152   std::cout << newDp << std::endl;
0153   //
0154   // prediction of variations with jacobian
0155   //
0156   AlgebraicVector5 dp;
0157   dp(0) = qpmod - 1. / tpg0.momentum().mag();
0158   dp(1) = dp(2) = dp(3) = dp(4) = 0;
0159   AlgebraicVector5 dpPred = jacobianL2L * dp;
0160   std::cout << dpPred << std::endl;
0161   //
0162   // comparison with difference in parameters
0163   //
0164   std::cout << newDx.x() - dpPred[3] << " " << newDx.y() - dpPred[4] << std::endl;
0165   std::cout << newDp.x() - dpPred[1] << " " << newDp.y() - dpPred[2] << std::endl;
0166   std::cout << (1. / ap2.mag() - 1. / ap.mag()) - dpPred[0] << std::endl;
0167
0168   return 0;
0169 }