example3.cpp

Go to the documentation of this file.

/*
 * example3.cpp
 *
 *  Created on: Aug 24, 2011
 *      Author: kleinwrt
 */
 
#include <time.h>
#include "GblUtilities.h"
#include "GblTrajectory.h"
 
using namespace gbl;
using namespace Eigen;
 
void example3() {
 
        unsigned int nTry = 1000; //: number of tries
        unsigned int nLayer = 10; //: number of detector layers
        std::cout << " Gbltst-eigen $Id$ " << nTry << ", " << nLayer
                        << std::endl;
 
        srand(4711);
 
        clock_t startTime = clock();
// track direction
        double sinLambda = 0.3;
        double cosLambda = sqrt(1.0 - sinLambda * sinLambda);
        double sinPhi = 0.;
        double cosPhi = sqrt(1.0 - sinPhi * sinPhi);
// tDir = (cosLambda * cosPhi, cosLambda * sinPhi, sinLambda)
// U = Z x T / |Z x T|, V = T x U
        Matrix<double, 2, 3> uvDir;
        uvDir(0, 0) = -sinPhi;
        uvDir(0, 1) = cosPhi;
        uvDir(0, 2) = 0.;
        uvDir(1, 0) = -sinLambda * cosPhi;
        uvDir(1, 1) = -sinLambda * sinPhi;
        uvDir(1, 2) = cosLambda;
// measurement resolution
        Vector2d measErr;
        measErr << 0.001, 0.001;
        Vector2d measPrec; // (independent) precisions
        measPrec << 1.0 / (measErr(0) * measErr(0)), 1.0 / (measErr(1) * measErr(1));
        Matrix2d measInvCov; // inverse covariance matrix
        measInvCov.setZero();
        measInvCov(0, 0) = measPrec[0];
        measInvCov(1, 1) = measPrec[1];
// scattering error
        Vector2d scatErr;
        scatErr << 0.003, 0.003;
        Vector2d scatPrec;
        scatPrec << 1.0 / (scatErr(0) * scatErr(0)), 1.0 / (scatErr(1) * scatErr(1));
// (RMS of) CurviLinear track parameters (Q/P, slopes, offsets)
        Vector5d clPar;
        Vector5d clErr;
        clErr << 0.001, -0.1, 0.2, -0.15, 0.25;
        Matrix5d clCov, clSeed;
        unsigned int seedLabel = 99999;
// additional parameters
        Vector2d scatVar;
        scatVar << 0., 0.; // measured scattering variance (in layer 4)
 
        double bfac = 0.2998; // Bz*c for Bz=1
        double step = 1.5 / cosLambda; // constant steps in RPhi
 
        double Chi2Sum = 0.;
        int NdfSum = 0;
        double LostSum = 0.;
        int numFit = 0;
 
        for (unsigned int iTry = 1; iTry <= nTry; ++iTry) {
                // curvilinear track parameters
                for (unsigned int i = 0; i < 5; ++i) {
                        clPar[i] = clErr[i] * unrm();
                }
                clCov.setZero();
                for (unsigned int i = 0; i < 5; ++i) {
                        clCov(i, i) = 1.0 * (clErr[i] * clErr[i]);
                }
//              std::cout << " Try " << iTry << ":" << clPar << std::endl;
// arclength
                double s = 0.;
                double sScat = 0.;
                Matrix5d jacPointToPoint;
                jacPointToPoint.setIdentity();
// create list of points
                std::vector<GblPoint> listOfPoints;
                listOfPoints.reserve(2 * nLayer);
 
                for (unsigned int iLayer = 0; iLayer < nLayer; ++iLayer) {
//                      std::cout << " Layer " << iLayer << ", " << s << std::endl;
//     measurement directions
                        double sinStereo = (iLayer % 2 == 0) ? 0. : 0.1;
                        double cosStereo = sqrt(1.0 - sinStereo * sinStereo);
                        Matrix<double, 3, 2> mDirT;
                        mDirT.setZero();
                        mDirT(1, 0) = cosStereo;
                        mDirT(2, 0) = sinStereo;
                        mDirT(1, 1) = -sinStereo;
                        mDirT(2, 1) = cosStereo;
// projection measurement to local (curvilinear uv) directions (duv/dm)
                        Matrix2d proM2l = uvDir * mDirT;
// projection local (uv) to measurement directions (dm/duv)
                        Matrix2d proL2m = proM2l.inverse();
                        // point with (independent) measurements (in measurement system)
                        GblPoint pointMeas(jacPointToPoint);
                        // measurement - prediction in measurement system with error
                        Vector2d meas = proL2m * clPar.tail(2);
                        for (unsigned int i = 0; i < 2; ++i) {
                                meas[i] += measErr[i] * unrm();
                        }
                        pointMeas.addMeasurement(proL2m, meas, measPrec);
                        /* point with (correlated) measurements (in local system)
                         GblPoint point(jacPointToPoint);
                         // measurement - prediction in local system with error
                         Vector2d meas;
                         for (unsigned int i = 0; i < 2; ++i) {
                         meas[i] = measErr[i] * unrm3();
                         }
                         meas = proM2l * meas + clPar.tail<2>();
                         Matrix2d localInvCov = proL2m.transpose() * measInvCov * proL2m;
                         point.addMeasurement(meas, localInvCov); */
 
                        // additional local parameters?
                        if (iLayer > 4) {
                                //std::cout << " scat " << sScat << " " << s << std::endl;
                                Matrix2d addDer = proL2m * (s - sScat);
                                pointMeas.addLocals(addDer);
                        }
 
// add point to trajectory
                        listOfPoints.push_back(pointMeas);
                        unsigned int iLabel = listOfPoints.size();
                        if (iLabel == seedLabel) {
                                clSeed = clCov.inverse();
                        }
// propagate to scatterer
                        jacPointToPoint = gblSimpleJacobian(step, cosLambda, bfac);
                        clPar = jacPointToPoint * clPar;
                        clCov = jacPointToPoint * clCov * jacPointToPoint.transpose();
                        s += step;
                        if (iLayer < nLayer - 1) {
                                Vector2d scat(0., 0.);
                                // point with scatterer
                                GblPoint pointScat(jacPointToPoint);
                                if (iLayer != 4) {
                                        pointScat.addScatterer(scat, scatPrec);
                                } else {
                                        // measure scattering (with 2 local parameters, no scatterer)
                                        sScat = s;
                                }
                                listOfPoints.push_back(pointScat);
                                iLabel = listOfPoints.size();
                                if (iLabel == seedLabel) {
                                        clSeed = clCov.inverse();
                                }
                                // scatter a little
                                for (unsigned int i = 0; i < 2; ++i) {
                                        clPar[i + 1] += scatErr[i] * unrm();
                                        clCov(i + 1, i + 1) += scatErr[i] * scatErr[i];
                                }
                                // propagate to next measurement layer
                                clPar = jacPointToPoint * clPar;
                                clCov = jacPointToPoint * clCov * jacPointToPoint.transpose();
                                s += step;
                        }
                }
//
                // create trajectory
                GblTrajectory traj(listOfPoints);
                //GblTrajectory traj(listOfPoints, seedLabel, clSeed); // with external seed
                //traj.printPoints();
 
                if (not traj.isValid()) {
                        std::cout << " Invalid GblTrajectory -> skip" << std::endl;
                        continue;
                }
// fit trajectory
                double Chi2;
                int Ndf;
                double lostWeight;
                traj.fit(Chi2, Ndf, lostWeight);
//              std::cout << " Fit: " << Chi2 << ", " << Ndf << ", " << lostWeight << std::endl;
                VectorXd aCorrection(7);
                MatrixXd aCovariance(7, 7);
                traj.getResults(10, aCorrection, aCovariance);
                // measured scattering parameters
                scatVar(0) += aCorrection(5) * aCorrection(5);
                scatVar(1) += aCorrection(6) * aCorrection(6);
                // contribution from fit
                scatVar(0) -= aCovariance(5, 5);
                scatVar(1) -= aCovariance(6, 6);
                /* look at residuals
                 for (unsigned int label = 1; label <= listOfPoints.size(); ++label) {
                 unsigned int numData = 0;
                 std::cout << " measResults, label " << label << std::endl;
                 VectorXd residuals(2), measErr(2), resErr(2), downWeights(2);
                 traj.getMeasResults(label, numData, residuals, measErr, resErr,
                 downWeights);
                 std::cout << " measResults, numData " << numData << std::endl;
                 for (unsigned int i = 0; i < numData; ++i) {
                 std::cout << " measResults " << label << " " << i << " "
                 << residuals[i] << " " << measErr[i] << " " << resErr[i]
                 << std::endl;
                 }
                 } */
// debug printout
                //traj.printTrajectory(1);
                //traj.printPoints(1);
                //traj.printData();
                Chi2Sum += Chi2;
                NdfSum += Ndf;
                LostSum += lostWeight;
                numFit++;
        }
 
        clock_t endTime = clock();
        double diff = endTime - startTime;
        double cps = CLOCKS_PER_SEC;
        std::cout << " Time elapsed " << diff / cps << " s" << std::endl;
        std::cout << " Chi2/Ndf = " << Chi2Sum / NdfSum << std::endl;
        std::cout << " Tracks fitted " << numFit << std::endl;
        std::cout << " ScatErr4 " << sqrt(scatVar[0] / numFit) << " "
                        << sqrt(scatVar[1] / numFit) << std::endl;
        if (LostSum > 0.)
                std::cout << " Weight lost   " << LostSum << std::endl;
}