CbmRichRingFitterRobustCOP.cxx

Go to the documentation of this file.

 
#include "CbmRichRingFitterRobustCOP.h"
 
#include <cmath>
#include <iostream>
 
using std::cout;
using std::endl;
using std::fabs;
using std::sqrt;
 
CbmRichRingFitterRobustCOP::CbmRichRingFitterRobustCOP() {}
 
CbmRichRingFitterRobustCOP::~CbmRichRingFitterRobustCOP() {}
 
void CbmRichRingFitterRobustCOP::DoFit(CbmRichRingLight* ring) {
  int nofHits = ring->GetNofHits();
 
  double radius  = 0.;
  double centerX = 0.;
  double centerY = 0.;
 
  if (nofHits < 3) {
    ring->SetCenterX(0.);
    ring->SetCenterY(0.);
    ring->SetRadius(0.);
    return;
  }
 
  int iter, iterMax        = 20;
  int i_iter, i_max_Robust = 4;
  const int MinNuberOfHits = 3;
  const int MaxNuberOfHits = 2000;
  double Xi, Yi, Zi;
  double M0, Mx, My, Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, Mxz2, Myz2,
    Cov_xy;  //,temp;
  double A0, A1, A2, A22, epsilon   = 0.000000000001;
  double Dy, xnew, xold, ynew, yold = 100000000000.;
  double GAM, DET;
  double SumS1 = 0., SumS2 = 0.;
  double sigma;
  double ctsigma;
  double sigma_min = 0.05;
  double dx, dy;
  double d[MaxNuberOfHits];
  double w[MaxNuberOfHits];
  double ct = 7.;
 
  for (int i = 0; i < MaxNuberOfHits; i++)
    w[i] = 1.;
 
  Mx = My = 0.;
  for (int i = 0; i < nofHits; i++) {
    Mx += ring->GetHit(i).fX;
    My += ring->GetHit(i).fY;
  }
 
  M0 = nofHits;
  Mx /= M0;
  My /= M0;
 
  for (i_iter = 0; i_iter < i_max_Robust; i_iter++) {
    sigma = sigma_min;
    if (i_iter != 0) {
      for (int i = 0; i < nofHits; i++) {
        dx   = Mx - ring->GetHit(i).fX;
        dy   = My - ring->GetHit(i).fY;
        d[i] = sqrt(dx * dx + dy * dy) - radius;
        SumS1 += w[i] * d[i] * d[i];
        SumS2 += w[i];
      }
      if (SumS2 == 0.) {
        sigma = sigma_min;
      } else {
        sigma = sqrt(SumS1 / SumS2);
      }
      if (sigma < sigma_min) sigma = sigma_min;
      ctsigma = ct * sigma;
      SumS1   = 0.;
      SumS2   = 0.;
      for (int i = 0; i < nofHits; i++) {
        if (d[i] <= ctsigma) {
          w[i] = (1 - (d[i] / (ctsigma)) * (d[i] / (ctsigma)))
                 * (1 - (d[i] / (ctsigma)) * (d[i] / (ctsigma)));
        } else {
          w[i] = 0.;
        }
      }
    }
    //computing moments (note: all moments are normed, i.e. divided by N)
    M0  = 0;
    Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
    for (int i = 0; i < nofHits; i++) {
      if (w[i] != 0.) {
        Xi = ring->GetHit(i).fX - Mx;
        Yi = ring->GetHit(i).fY - My;
        Zi = Xi * Xi + Yi * Yi;
        Mxy += Xi * Yi;
        Mxx += Xi * Xi;
        Myy += Yi * Yi;
        Mxz += Xi * Zi;
        Myz += Yi * Zi;
        Mzz += Zi * Zi;
        M0++;
      }
    }
    if (M0 < MinNuberOfHits) {
      M0  = 0;
      Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
      M0                                = 0;
      for (int i = 0; i < nofHits; i++) {
        Xi = ring->GetHit(i).fX - Mx;
        Yi = ring->GetHit(i).fY - My;
        Zi = Xi * Xi + Yi * Yi;
        Mxy += Xi * Yi;
        Mxx += Xi * Xi;
        Myy += Yi * Yi;
        Mxz += Xi * Zi;
        Myz += Yi * Zi;
        Mzz += Zi * Zi;
        M0++;
      }
    }
    Mxx /= M0;
    Myy /= M0;
    Mxy /= M0;
    Mxz /= M0;
    Myz /= M0;
    Mzz /= M0;
 
    //computing the coefficients of the characteristic polynomial
    Mz     = Mxx + Myy;
    Cov_xy = Mxx * Myy - Mxy * Mxy;
    Mxz2   = Mxz * Mxz;
    Myz2   = Myz * Myz;
 
    A2 = 4. * Cov_xy - 3. * Mz * Mz - Mzz;
    A1 = Mzz * Mz + 4. * Cov_xy * Mz - Mxz2 - Myz2 - Mz * Mz * Mz;
    A0 = Mxz2 * Myy + Myz2 * Mxx - Mzz * Cov_xy - 2. * Mxz * Myz * Mxy
         + Mz * Mz * Cov_xy;
 
    A22  = A2 + A2;
    iter = 0;
    xnew = 0.;
 
    //Newton's method starting at x=0
    for (iter = 0; iter < iterMax; iter++) {
      ynew = A0 + xnew * (A1 + xnew * (A2 + 4. * xnew * xnew));
 
      if (fabs(ynew) > fabs(yold)) {
        //  printf("Newton2 goes wrong direction: ynew=%f  yold=%f\n",ynew,yold);
        xnew = 0.;
        break;
      }
 
      Dy   = A1 + xnew * (A22 + 16. * xnew * xnew);
      xold = xnew;
      xnew = xold - ynew / Dy;
 
      if (fabs((xnew - xold) / xnew) < epsilon) break;
    }
 
    if (iter == iterMax - 1) {
      //printf("Newton2 does not converge in %d iterations\n",iterMax);
      xnew = 0.;
    }
    if (xnew < 0.) {
      iter = 30;
      //printf("Negative root:  x=%f\n",xnew);
    }
 
    // computing the circle parameters
    GAM     = -Mz - xnew - xnew;
    DET     = xnew * xnew - xnew * Mz + Cov_xy;
    centerX = (Mxz * (Myy - xnew) - Myz * Mxy) / DET / 2.;
    centerY = (Myz * (Mxx - xnew) - Mxz * Mxy) / DET / 2.;
    radius  = sqrt(centerX * centerX + centerY * centerY - GAM);
    centerX = centerX + Mx;
    centerY = centerY + My;
    Mx      = centerX;
    My      = centerY;
 
    if (DET == 0.) {
      radius  = 0.;
      centerX = 0.;
      centerY = 0.;
      return;
    }
  }
  ring->SetRadius(radius);
  ring->SetCenterX(centerX);
  ring->SetCenterY(centerY);
 
  CalcChi2(ring);
}