PixelTrackFitting/test/testEigenJacobian.cpp

0001 #include "RecoTracker/PixelTrackFitting/interface/FitUtils.h"
0002 #include <cmath>
0003
0004 using riemannFit::Matrix5d;
0005 using riemannFit::Vector5d;
0006
0007 #include "TrackingTools/AnalyticalJacobians/interface/JacobianLocalToCurvilinear.h"
0008
0009 #include "DataFormats/GeometrySurface/interface/Surface.h"
0010 #include "DataFormats/TrajectoryState/interface/LocalTrajectoryParameters.h"
0011 #include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
0012
0013 #include "DataFormats/GeometrySurface/interface/Plane.h"
0014
0015 #include "MagneticField/Engine/interface/MagneticField.h"
0016
0017 namespace {
0018
0019   struct M5T : public MagneticField {
0020     M5T() : mf(0., 0., 5.) {}
0021     virtual GlobalVector inTesla(const GlobalPoint&) const { return mf; }
0022
0023     GlobalVector mf;
0024   };
0025
0026 }  // namespace
0027
0028 // old pixeltrack version...
0029 Matrix5d transfFast(Matrix5d cov, Vector5d const& p) {
0030   auto sqr = [](auto x) { return x * x; };
0031   auto sinTheta = 1 / std::sqrt(1 + p(3) * p(3));
0032   auto cosTheta = p(3) * sinTheta;
0033   cov(2, 2) = sqr(sinTheta) * (cov(2, 2) * sqr(1. / (p(2) * p(2))) + cov(3, 3) * sqr(cosTheta * sinTheta / p(2)));
0034   cov(3, 2) = cov(2, 3) = cov(3, 3) * cosTheta * sqr(sinTheta) / p(2);
0035   // for (int i=0; i<5; ++i) cov(i,2) *= -sinTheta/(p(2)*p(2));
0036   // for (int i=0; i<5; ++i) cov(2,i) *= -sinTheta/(p(2)*p(2));
0037   return cov;
0038 }
0039
0040 Matrix5d loadCov(Vector5d const& e) {
0041   Matrix5d cov;
0042   for (int i = 0; i < 5; ++i)
0043     cov(i, i) = e(i) * e(i);
0044   for (int i = 0; i < 5; ++i) {
0045     for (int j = 0; j < i; ++j) {
0046       double v = 0.3 * std::sqrt(cov(i, i) * cov(j, j));  // this makes the matrix pos defined
0047       cov(i, j) = (i + j) % 2 ? -0.4 * v : 0.1 * v;
0048       cov(j, i) = cov(i, j);
0049     }
0050   }
0051   return cov;
0052 }
0053
0054 #include <iostream>
0055 int main() {
0056   M5T const mf;
0057
0058   for (auto charge = -1; charge < 2; charge += 2)
0059     for (auto szip = -1; szip < 2; szip += 2)
0060       for (auto stip = -1; stip < 2; stip += 2) {
0061         Vector5d par0;
0062         par0 << 0.2, 0.1, 3.5, 0.8, 0.1;
0063         Vector5d del0;
0064         del0 << 0.01, 0.01, 0.035, -0.03, -0.01;
0065         //!<(phi,Tip,pt,cotan(theta)),Zip)
0066         par0(1) *= stip;
0067         par0(4) *= szip;
0068
0069         Matrix5d cov0 = loadCov(del0);
0070
0071         Vector5d par1;
0072         Vector5d par2;
0073
0074         Matrix5d cov1;
0075         Matrix5d cov2;
0076
0077         // Matrix5d covf = transfFast(cov0,par0);
0078
0079         riemannFit::transformToPerigeePlane(par0, cov0, par1, cov1);
0080
0081         std::cout << "cov1\n" << cov1 << std::endl;
0082
0083         LocalTrajectoryParameters lpar(par1(0), par1(1), par1(2), par1(3), par1(4), 1.);
0084         AlgebraicSymMatrix55 m;
0085         for (int i = 0; i < 5; ++i)
0086           for (int j = i; j < 5; ++j)
0087             m(i, j) = cov1(i, j);
0088
0089         float phi = par0(0);
0090         float sp = std::sin(phi);
0091         float cp = std::cos(phi);
0092         Surface::RotationType rot(sp, -cp, 0, 0, 0, -1.f, cp, sp, 0);
0093
0094         Surface::PositionType bs(0., 0., 0.);
0095         Plane plane(bs, rot);
0096         GlobalTrajectoryParameters gp(
0097             plane.toGlobal(lpar.position()), plane.toGlobal(lpar.momentum()), lpar.charge(), &mf);
0098         std::cout << "global par " << gp.position() << ' ' << gp.momentum() << ' ' << gp.charge() << std::endl;
0099         JacobianLocalToCurvilinear jl2c(plane, lpar, mf);
0100         std::cout << "jac l2c" << jl2c.jacobian() << std::endl;
0101
0102         AlgebraicSymMatrix55 mo = ROOT::Math::Similarity(jl2c.jacobian(), m);
0103         std::cout << "curv error\n" << mo << std::endl;
0104
0105         /*
0106
0107   // not accurate as the perigee plane move as well...
0108   Vector5d del1 = par2-par1;
0109
0110
0111   // don't ask: guess
0112   std::cout << "charge " << charge << std::endl;
0113   std::cout << "par0 " << par0.transpose() << std::endl;
0114   std::cout << "del0 " << del0.transpose() << std::endl;
0115
0116
0117   std::cout << "par1 " << par1.transpose() << std::endl;
0118   std::cout << "del1 " << del1.transpose() << std::endl;
0119   // std::cout << "del2 " << (J*del0).transpose() << std::endl;
0120
0121   std::cout << "del1^2 " << (del1.array()*del1.array()).transpose() << std::endl;
0122   std::cout << std::endl;
0123
0124   std::cout << "cov0\n" << cov0 << std::endl;
0125   std::cout << "cov1\n" << cov1 << std::endl;
0126   std::cout << "cov2\n" << cov2 << std::endl;
0127   */
0128
0129         std::cout << std::endl << "----------" << std::endl;
0130
0131       }  // lopp over signs
0132
0133   return 0;
0134 }