PixelTrackFitting/src/ParabolaFit.cc

0001 #include "ParabolaFit.h"
0002 #include <iostream>
0003 using namespace std;
0004 template <class T>
0005 T sqr(T t) {
0006   return t * t;
0007 }
0008
0009 void ParabolaFit::addPoint(double x, double y) {
0010   hasWeights = false;
0011   addPoint(x, y, 1.);
0012 }
0013
0014 void ParabolaFit::addPoint(double x, double y, double w) {
0015   hasValues = false;
0016   hasErrors = false;
0017   Point p = {x, y, w};
0018   points.push_back(p);
0019 }
0020
0021 const ParabolaFit::Result& ParabolaFit::result(bool doErrors) const {
0022   if (hasErrors)
0023     return theResult;
0024   if (hasValues && !doErrors)
0025     return theResult;
0026
0027   double F0, F1, F2, F3, F4, F0y, F1y, F2y;
0028   F0 = F1 = F2 = F3 = F4 = F0y = F1y = F2y = 0.;
0029
0030   typedef vector<Point>::const_iterator IT;
0031   for (IT ip = points.begin(); ip != points.end(); ip++) {
0032     double pow;
0033     double x = ip->x;
0034     double y = ip->y;
0035     double w = ip->w;
0036
0037     F0 += w;
0038     F0y += w * y;
0039     F1 += w * x;
0040     F1y += w * x * y;
0041     pow = x * x;
0042     F2 += w * pow;
0043     F2y += w * pow * y;  // x^2
0044     pow *= x;
0045     F3 += w * pow;  // x^3
0046     pow *= x;
0047     F4 += w * pow;  // x^4
0048   }
0049
0050   Column cA = {F0, F1, F2};
0051   Column cB = {F1, F2, F3};
0052   Column cC = {F2, F3, F4};
0053   Column cY = {F0y, F1y, F2y};
0054
0055   double det0 = det(cA, cB, cC);
0056
0057   if (!hasFixedParC) {
0058     theResult.parA = det(cY, cB, cC) / det0;
0059     theResult.parB = det(cA, cY, cC) / det0;
0060     theResult.parC = det(cA, cB, cY) / det0;
0061   } else {
0062     Column cCY = {F0y - theResult.parC * F2, F1y - theResult.parC * F3, F2y - theResult.parC * F4};
0063     double det0C = det(cA, cB);
0064     theResult.parA = det(cCY, cB) / det0C;
0065     theResult.parB = det(cA, cCY) / det0C;
0066   }
0067
0068   //  std::cout <<" result: A="<<theResult.parA<<" B="<<theResult.parB<<" C="<<theResult.parC<<endl;
0069   double vAA, vBB, vCC, vAB, vAC, vBC;
0070   vAA = vBB = vCC = vAB = vAC = vBC = 0.;
0071
0072   hasValues = true;
0073   if (!doErrors)
0074     return theResult;
0075
0076   if (!hasWeights && dof() > 0) {
0077     //     cout <<" CHI2: " << chi2() <<" DOF: " << dof() << endl;
0078     double scale_w = 1. / chi2() / dof();
0079     for (IT ip = points.begin(); ip != points.end(); ip++)
0080       ip->w *= scale_w;
0081     //     cout <<" CHI2: " << chi2() <<" DOF: " << dof() << endl;
0082   }
0083
0084   for (IT ip = points.begin(); ip != points.end(); ip++) {
0085     double w = ip->w;
0086     Column cX = {1., ip->x, sqr(ip->x)};
0087
0088     double dXBC = det(cX, cB, cC);
0089     double dAXC = det(cA, cX, cC);
0090     double dABX = det(cA, cB, cX);
0091
0092     vAA += w * sqr(dXBC);
0093     vBB += w * sqr(dAXC);
0094     vCC += w * sqr(dABX);
0095     vAB += w * dXBC * dAXC;
0096     vAC += w * dXBC * dABX;
0097     vBC += w * dAXC * dABX;
0098   }
0099
0100   theResult.varAA = vAA / sqr(det0);
0101   theResult.varBB = vBB / sqr(det0);
0102   theResult.varCC = vCC / sqr(det0);
0103   theResult.varAB = vAB / sqr(det0);
0104   theResult.varAC = vAC / sqr(det0);
0105   theResult.varBC = vBC / sqr(det0);
0106
0107   hasErrors = true;
0108   return theResult;
0109 }
0110
0111 double ParabolaFit::chi2() const {
0112   double mychi2 = 0.;
0113   for (vector<Point>::const_iterator ip = points.begin(); ip != points.end(); ip++) {
0114     mychi2 += ip->w * sqr(ip->y - fun(ip->x));
0115   }
0116   return mychi2;
0117 }
0118
0119 double ParabolaFit::fun(double x) const { return theResult.parA + theResult.parB * x + theResult.parC * x * x; }
0120
0121 int ParabolaFit::dof() const {
0122   int nPar = 3;
0123   if (hasFixedParC)
0124     nPar--;
0125   int nDof = points.size() - nPar;
0126   return (nDof > 0) ? nDof : 0;
0127 }
0128
0129 double ParabolaFit::det(const Column& c1, const Column& c2, const Column& c3) const {
0130   return c1.r1 * c2.r2 * c3.r3 + c2.r1 * c3.r2 * c1.r3 + c3.r1 * c1.r2 * c2.r3 - c1.r3 * c2.r2 * c3.r1 -
0131          c2.r3 * c3.r2 * c1.r1 - c3.r3 * c1.r2 * c2.r1;
0132 }
0133
0134 double ParabolaFit::det(const Column& c1, const Column& c2) const { return c1.r1 * c2.r2 - c1.r2 * c2.r1; }