CRingFitter.C
//-----------------------------------------------------------------------------
// $Header: CRingFitter.C,v 3.1 97/04/18 19:20:52 messer Exp $
//
// COOL Program Library
// Copyright (C) CERES collaboration, 1996
//
// Implementation of class CRingFitter.
//
//-----------------------------------------------------------------------------
#include <iostream.h>
#include <iomanip.h>
#include "CRingFitter.h"
CRingFitter::CRingFitter()
{
this->resetAll();
}
CRingFitter::~CRingFitter() {}
void CRingFitter::reset()
{
x.clear();
y.clear();
w.clear();
res.clear();
resold.clear();
}
void CRingFitter::resetAll()
{
this->reset();
valTukey = 0.4;
maxIter = 20;
radius = 0;
xCenter = 0;
yCenter = 0;
variance = 0;
ret = 0;
iter = maxIter;
secondXCenter = 0;
secondYCenter = 0;
}
void CRingFitter::setWeights(double weight)
{
w.clear();
for (int i=0; i<x.entries(); i++)
w.insert(weight);
}
void CRingFitter::printStatistics(ostream& ost) const
{
CString line('=', 46);
switch (algorithm) {
case Chernov:
ost << "\nStatistics on Chernov ring fit" << endl;
break;
case RobustFixedRadius:
ost << "\nStatistics on robust single ring fit (fixed radius)" << endl;
break;
case RobustDoubleRing:
ost << "\nStatistics on robust double ring fit (fixed radius)" << endl;
break;
default:
ost << "\nStatistics on ring fit" << endl;
break;
}
ost << line << endl;
ost << "Number of fit points: " << x.length() << endl;
ost << '\t' << "x" << '\t' << "y" << '\t' << "weight" << endl;
for (int i=0; i<x.length(); i++)
ost << '\t' << x(i) << '\t' << y(i) << '\t' << w(i) << endl;
ost << "Ring center: " << '(' << xCenter << ", " << yCenter << ')' << endl;
if (algorithm == RobustDoubleRing)
ost << " " << '(' << secondXCenter << ", " << secondYCenter << ')' << endl;
ost << "Radius: " << radius << endl;
ost << "Fit variance: " << variance << endl;
ost << "Iterations: " << this->getNIter() << endl;
ost << "Error code: " << ret << endl;
}
//
// Chernov/Ososkov fit
//
// Fast fitting routine using a iterational linear regression
// method (ILRM). Reference: N.Chernov, G.A.Ososkov Computer
// Physics Communication 33 (1984) 329-333.
//
CBoolean CRingFitter::chernov()
{
algorithm = Chernov;
int i;
double xx, yy, xx2, yy2;
double f, g, h, p, q, t, g0, g02, a, b, c, d;
double xroot, ff, fp, xd, yd, g1;
double dx, dy, dradius2, xnom;
double xgravity = 0.0;
double ygravity = 0.0;
double x2 = 0.0;
double y2 = 0.0;
double xy = 0.0;
double xx2y2 = 0.0;
double yx2y2 = 0.0;
double x2y22 = 0.0;
double radius2 = 0.0;
int npoints = x.length();
if (npoints <= 3) {
ret = 1;
return False;
}
for (i=0; i<npoints; i++) {
xgravity += x[i];
ygravity += y[i];
}
xgravity /= npoints;
ygravity /= npoints;
for (i=0; i<npoints; i++) {
xx = x[i]-xgravity;
yy = y[i]-ygravity;
xx2 = xx*xx;
yy2 = yy*yy;
x2 += xx2;
y2 += yy2;
xy += xx*yy;
xx2y2 += xx*(xx2+yy2);
yx2y2 += yy*(xx2+yy2);
x2y22 += (xx2+yy2)*(xx2+yy2);
}
if (xy == 0.) {
ret = 2;
return False;
}
f = (3.*x2+y2)/npoints;
g = (x2+3.*y2)/npoints;
h = 2*xy/npoints;
p = xx2y2/npoints;
q = yx2y2/npoints;
t = x2y22/npoints;
g0 = (x2+y2)/npoints;
g02 = g0*g0;
a = -4.0;
b = (f*g-t-h*h)/g02;
c = (t*(f+g)-2.*(p*p+q*q))/(g02*g0);
d = (t*(h*h-f*g)+2.*(p*p*g+q*q*f)-4.*p*q*h)/(g02*g02);
xroot = 1.0;
for (i=0; i<5; i++) {
ff = (((xroot+a)*xroot+b)*xroot+c)*xroot+d;
fp = ((4.*xroot+3.*a)*xroot+2.*b)*xroot+c;
xroot -= ff/fp;
}
g1 = xroot*g0;
xnom = (g-g1)*(f-g1)-h*h;
if (xnom == 0.) {
ret = 3;
return False;
}
yd = (q*(f-g1)-h*p)/xnom;
xnom = f-g1;
if (xnom == 0.) {
ret = 4;
return False;
}
xd = (p-h*yd )/xnom;
radius2 = xd*xd+yd*yd+g1;
xCenter = xd+xgravity;
yCenter = yd+ygravity;
for (i=0; i<npoints; i++) {
dx = x[i]-(xCenter);
dy = y[i]-(yCenter);
dradius2 = dx*dx+dy*dy;
variance += dradius2+radius2-2.*sqrt(dradius2*radius2);
}
variance /= npoints-3.0;
radius = sqrt(radius2);
return True;
}
//
// Robust ring fits with fixed radius
// Method developed by G.A.Ososkov.
//
CBoolean CRingFitter::robustFixedRadius()
{
algorithm = RobustFixedRadius;
const double eps = 0.01;
int i;
int npos = 0;
double xcen = 0;
double ycen = 0;
double sumw = 0;
//
// Calculate the residual vector and initial value of the variance
// using Tukey weight values
//
double cvar = 1.5;
double sumres = 0;
double rr = radius*radius;
double val;
double a = xCenter;
double b = yCenter;
sumw = 0;
for (i=0; i<x.length(); i++) {
val = sqrt( (x(i)-a)*(x(i)-a) + (y(i)-b)*(y(i)-b) ) - radius;
if (fabs(val) >= cvar)
w(i) = 0;
else {
w(i) = 1.-(val/cvar)*(val/cvar);
w(i) *= w(i);
}
sumres += val*val*w(i);
sumw += w(i);
res.insert(val);
resold.insert(val);
}
if (fabs(sumw) < DBL_EPSILON) {
ret = 3;
return False;
}
variance = sumres/sumw;
double del;
double ar, sumr;
double var_0 = 0.25;
iter = maxIter;
//
// Iterate until precision is reached
//
while (iter--) {
if (!fitFixedRadius()) break;
if ( (variance < 1.e-2) || (ret != 0) ) break;
del = 0;
//
// Updating weights by the formula for the optimal weights ('ideal weights')
//
cvar = valTukey * sqrt(var_0); // variable width
sumw = 0.;
for (i=0; i<x.length(); i++) {
if(fabs(res(i)) < 5.) {
w(i) = (1.+cvar)/(1.+cvar*exp(res(i)*res(i)/(2.*var_0)));
sumw += w(i);
}
else
w(i) = 0;
ar = fabs(res(i)-resold(i));
if (del < ar) del = ar;
resold(i) = res(i);
}
if (del < eps) {
sumr=0.;
for (i=0; i<x.length(); i++)
if(w(i) > 0) sumr += w(i)*res(i);
if (fabs(sumw) < DBL_EPSILON) {
ret = 3;
return False;
}
radius = sumr/sumw + radius;
break;
}
}
if (ret != 0)
return False;
else
return True;
}
CBoolean CRingFitter::fitFixedRadius()
{
const double eps = DBL_EPSILON;
int i;
if (radius < FLT_EPSILON) { // radius not set or too small
xCenter = 0;
yCenter = 0;
variance = 9999.;
ret = 42;
cerr << "CRingFitter::fitFixedRadius()" << endl;
cerr << "\tERROR" << endl;
cerr << "\tRadius not set or too small. No fit performed." << endl;
return False;
}
double c3 = 3.;
double xx, yy, xy, f, g, h, p, q, t, det;
double abr, ff, gg, s, da, db, resval;
double xx2, yy2, sumres = 0;
double xi, yi;
double di;
int npos = 0; // initialize
double xcen = 0;
double ycen = 0;
double sumw = 0;
ret = 0;
for (i=0; i<x.length(); i++) { // calculate center of gravity
xcen += x(i) * w(i);
ycen += y(i) * w(i);
sumw += w(i);
if (w(i) > eps) npos++;
}
if (fabs(sumw) < DBL_EPSILON) {
ret = 3;
return False;
}
xcen /= sumw;
ycen /= sumw;
if (npos < 3) { // not enough points for a fit
xCenter = xcen;
yCenter = ycen;
variance= 9999.;
ret = 1;
return False;
}
double a = xCenter;
double b = yCenter;
//
// Calculate the gauss-bracket values
//
xx = 0.; yy = 0.; xy = 0.;
p = 0.; q = 0.;
for (i=0; i<x.length(); i++) {
xi = x(i)-xcen;
yi = y(i)-ycen;
xx2 = xi*xi;
yy2 = yi*yi;
di = xx2+yy2;
xx = xx+xx2*w(i);
yy = yy+yy2*w(i);
xy = xy+xi*yi*w(i);
p = p+xi*di*w(i);
q = q+yi*di*w(i);
}
a -= xcen;
b -= ycen;
//
// Calculate the equation coefficients
//
abr = a*a+b*b-radius*radius;
ff = c3*xx+yy;
gg = xx+c3*yy;
h = sumw*(abr+2.*a*a)+ff;
f = 2.*(xy+a*b*sumw);
g = sumw*(abr+2.*b*b)+gg;
det = h*g-f*f;
if(fabs(det) < eps) { // determinant too small
xCenter = a;
yCenter = b;
variance= 9999.;
ret = 2;
return False;
}
s = p-a*(ff+sumw*abr)-2.*b*xy;
t = q-2.*a*xy-b*(gg+sumw*abr);
//
// Solve the linear equation
//
da=(s*g-t*f)/det;
db=(t*h-s*f)/det;
//
// Calculate the circle parameters
//
a = a+da+xcen;
b = b+db+ycen;
xCenter = a;
yCenter = b;
//
// Calculate the residual vector
//
for (i=0; i<x.length(); i++) {
resval = sqrt((x(i)-a)*(x(i)-a)+(y(i)-b)*(y(i)-b))-radius;
sumres = sumres+resval*resval*w(i);
res(i) = resval;
}
//
// Estimate the variance
//
variance = sumres/sumw;
return True;
}
//
// Double ring fitting routine for fixed radius
// Author: H.Agakishiev, G.Ososkov (1995)
//
CBoolean CRingFitter::robustDoubleRing()
{
algorithm = RobustDoubleRing;
const double maxStep = 10.0; // maximal step of parameter change
const double minStep = maxStep/100; // minimal step of parameter change
valTukey = 3.;
double width;
double sumD2W;
double sumW;
double fk[4][4];
double ff[4];
double f[5];
double xa, xc, yb, yd;
double xa2, xc2, yb2, yd2, r2;
double distRing1, distRing2, dist;
double step, weight;
int i, j, k, npoints;
//
// Initialize fit
//
int nFitPoints = x.length();
for (i=0; i<4; i++) {
ff[i] = 0;
for (j=0; j<4; j++) fk[i][j] = 0;
}
for (i=0; i<5; i++) f[i] = 0;
ret = 0;
iter = maxIter;
//
// Iterate until requested precision is reached
//
width = valTukey*3;
while (iter--) {
//
// Evaluate coefficiences and weights from data points
//
npoints = 0;
sumD2W = 0;
sumW = 0;
for (i=0; i<nFitPoints; i++) {
xa = x[i] - xCenter;
xc = x[i] - secondXCenter;
yb = y[i] - yCenter;
yd = y[i] - secondYCenter;
xa2 = xa*xa;
xc2 = xc*xc;
yb2 = yb*yb;
yd2 = yd*yd;
r2 = radius*radius;
distRing1 = xa2 + yb2 - r2; // squared distance between this point and rings
distRing2 = xc2 + yd2 - r2;
dist = fabs(distRing1) > fabs(distRing2) ? sqrt(fabs(distRing2)) : sqrt(fabs(distRing1));
sumW += w[i];
sumD2W += dist*dist*w[i];
//
// Calculate Tukey weight (is wrong in f90 version: no iteration ...)
//
if (dist <= width) {
weight = 1. - (dist/width)*(dist/width);
weight *= weight;
npoints++;
}
else
weight = 0;
w[i] = weight;
//
// Calculate coefficiences of polynom
//
f[0] = -2*xa*distRing2;
f[1] = -2*yb*distRing2;
f[2] = -2*xc*distRing1;
f[3] = -2*yd*distRing1;
f[4] = distRing1*distRing2;
for (j=0; j<4; j++) {
ff[j] -= f[4]*f[j]*weight;
for (k=0; k<=j; k++)
fk[j][k] += f[j]*f[k]*weight;
}
}
if (npoints < 6) {
ret = 1;
return False;
}
if (sumW == 0) {
ret = 10;
return False;
}
width = valTukey*sumD2W/sumW; // update width of Tukey potential
variance = sumD2W/sumW; // some measure of quality...
//
// Linear system solving
//
if (!symmax(fk, ff, 4)) {
ret = 3;
return False;
}
//
// Check maximal difference between old and new value of parameters
//
step = 0;
for (j=0; j<4; j++)
if (fabs(ff[j]) > step) step = ff[j] > 0 ? ff[j] : -ff[j];
if (step < minStep) return True; // all done
//
// Limitation of parameters change steps
//
for (j=0; j<4; j++)
if(fabs(ff[j]) > maxStep) ff[j] = ff[j] > 0 ? maxStep : -maxStep;
xCenter += ff[0];
yCenter += ff[1];
secondXCenter += ff[2];
secondYCenter += ff[3];
} // end while (iter--)
ret = 2;
return False;
}
//
// Free Radius Fit used for Pion ( "quite" exact copy of Fortran 90)
//
CBoolean CRingFitter::robustFreeRadius()
{
const double eps = 0.01; // it was 0.1 , TU. suggested to put 0.01
valTukey = 3.5; // for FixRadiusFit is smaller (= 0.4)
int i;
int npos = 0;
double xcen = 0;
double ycen = 0;
double sumw = 0;
//
// Calculate the residual vector and initial value of the variance
// using Tukey weight values
//
double cvar = valTukey*2;
double sumres = 0;
double rr = radius*radius;
double val;
double a = xCenter;
double b = yCenter;
sumw = 0;
for (i=0; i<x.length(); i++) {
val = sqrt( (x(i)-a)*(x(i)-a) + (y(i)-b)*(y(i)-b) ) - radius;
if (fabs(val) >= cvar)
w(i) = 0;
else {
w(i) = 1.-(val/cvar)*(val/cvar);
w(i) *= w(i);
}
sumres += val*val*w(i);
sumw += w(i);
res.insert(val);
resold.insert(val);
}
if (fabs(sumw) < DBL_EPSILON) {
ret = 3;
return False;
}
variance = sumres/sumw;
if (!fitFreeRadius()) return False;
if ( (variance < 1.e-10) ) return True;
//
// Iterate until precision is reached
//
double del;
double ar;
iter = 100;
while (iter--) {
//
// Updating weights by the formula for the optimal weights ('ideal weights')
//
cvar = valTukey * sqrt(variance); // variable width
if(cvar < 0.7) cvar = 0.7 ;
sumw = 0.;
for (i=0; i<x.length(); i++) {
if(fabs(resold(i)) >= cvar) {
w(i) = 0;
}
else {
w(i) = 1. - (resold(i)/cvar)*(resold(i)/cvar);
w(i) *= w(i);
sumw += w(i);
}
}
if (!fitFreeRadius()) break;
if ( (variance < 1.e-10) || ret == 1 || ret == 2 ) break;
del = 0.;
double dr2, rtwo, dx, dy;
double dvar = 0.;
for (i=0; i<x.length(); i++) {
dx = x(i) - xCenter;
dy = y(i) - yCenter;
rtwo = radius*radius;
dr2 = dx*dx+dy*dy;
dvar += dr2+rtwo-2.*sqrt(dr2*rtwo);
ar = fabs(res(i)-resold(i));
if (del < ar) del = ar;
resold(i) = res(i);
}
if (x.length() != 3.) {
dvar /= x.length() - 3.0;
}
if (del < eps) {
break;
}
}
if (ret == 1 || ret == 2)
return False;
else
return True;
}
CBoolean CRingFitter::fitFreeRadius()
{
const double eps = DBL_EPSILON;
int i;
int iterBis = 20;
if (radius < FLT_EPSILON) { // radius not set or too small
xCenter = 0;
yCenter = 0;
variance = 9999.;
ret = 42;
cerr << "CRingFitter::fitFreeRadius()" << endl;
cerr << "\tERROR" << endl;
cerr << "\tRadius not set or too small. No fit performed." << endl;
return False;
}
double c1 = 1.; // fortran 90 variables
double c2 = 2.;
double c4 = 4.;
double c12 = 12.;
double gamin, gam, fact;
double ao, a1, a2, a22, xa, ya, xb, dy;
double rr, r2;
double c3 = 3.;
double xx, yy, xy, f, g, h, p, q, t, det;
double xx2, yy2;
double xi, yi;
double di;
int npos = 0; // initialize
double xcen = 0.;
double ycen = 0.;
double sumw = 0.;
double sumres = 0.;
ret = 0;
for (i=0; i<x.length(); i++) { // calculate center of gravity
xcen += x(i) * w(i);
ycen += y(i) * w(i);
sumw += w(i);
if (w(i) > eps) npos++;
}
if (fabs(sumw) < DBL_EPSILON) {
ret = 3;
return False;
}
xcen /= sumw;
ycen /= sumw;
if (npos < 3) { // not enough points for a fit
xCenter = xcen;
yCenter = ycen;
variance= 9999.;
ret = 1;
return False;
}
double a = xCenter;
double b = yCenter;
//
// Calculate the gauss-bracket values
//
xx = 0.; yy = 0.; xy = 0.;
p = 0.; q = 0.; t = 0.;
for (i=0; i<x.length(); i++) {
xi = x(i)-xcen;
yi = y(i)-ycen;
xx2 = xi*xi;
yy2 = yi*yi;
di = xx2+yy2;
xx = xx+xx2*w(i);
yy = yy+yy2*w(i);
xy = xy+xi*yi*w(i);
p = p+xi*di*w(i);
q = q+yi*di*w(i);
t = t+di*di*w(i);
}
xx /= sumw;
yy /= sumw;
xy /= sumw;
p /= sumw;
q /= sumw;
t /= sumw;
//
// Calculate the equation coefficients
//
f = c3*xx+yy;
g = xx+c3*yy;
h = 2*xy;
gamin = xx + yy;
if(gamin < eps) { // determinant too small
xCenter = a;
yCenter = b;
variance= 9999.;
ret = 2;
return False;
}
fact = gamin*gamin;
a2 = (f*g - t - h*h)/fact;
a22 = a2 + a2;
fact = fact*gamin;
a1 = (t*(f+g) - 2.*(p*p+q*q))/fact;
fact = fact*gamin;
ao = (t*(h*h-f*g)+2.*(p*p*g+q*q*f)-4.*p*q*h)/fact;
//
// Solve the linear equation
//
xa = c1;
CBoolean resetxb = True;
while (iterBis--){
ya = ao + xa*(a1+xa*(a2+xa*(xa-c4)));
dy = a1+xa*(a22+xa*(c4*xa-c12));
if(fabs(dy) < eps) break;
xb = xa - ya/dy;
if(xb < eps) break;
if(fabs(xa-xb) < eps) {
resetxb = False;
break;
}
xa = xb;
}
if(resetxb) {
xb = c1;
ret = 3;
}
//
// Calculate the circle parameters
//
gam = gamin*xb;
det = (f-gam)*(g-gam) - h*h;
if(fabs(det) < eps) {
ret = 2;
return False;
}
a = (p*(g-gam) - q*h)/det;
b = (q*(f-gam) - p*h)/det;
radius = sqrt(gam + a*a + b*b);
a = a + xcen;
b = b + ycen;
xCenter = a;
yCenter = b;
//
// Calculate the residual vector
//
rr = radius*radius;
r2 = radius*c2;
for (i=0; i<x.length(); i++) {
res(i) = ((x(i)-a)*(x(i)-a)+(y(i)-b)*(y(i)-b)-rr)/r2;
sumres = sumres+res(i)*res(i)*w(i);
}
variance = sumres/sumw;
return True;
}
//
// This is a routine for solving a linear system with
// a symmetric matrix of coefficients.
//
// On input: A(n,n) - the matrix of coefficients,
// B(n) - the right-hand side vector of the system,
// n - the dimension of the matrices A and B.
//
// On output: B(n) - the vector of solutions.
//
// WARNINGS: 1) the matrices A and B will be modified by the routine;
// 2) the routine does not check that A is a symmetric
// matrix, this is for user's responsibility;
// 3) the routine uses the lower triangle of the matrix A only,
// so the user does not need to fill the entire matrix A,
// he must define the entries A[i][j] for i>=j only;
// 4) in the case of singularity of the matrix A the routine
// sends a message onto the screen and no longer evaluates
// the solution.
//
// Remarks: This function was originally written in FORTRAN-4 by
// H.Agakishiev and unreadable! I tried to keep as close
// to the original to not mix up the way this code works.
// Sorry, but is still better than the f2c version and is
// free of the 6 goto statements which were in before. tu.
//
CBoolean CRingFitter::symmax(double A[][4], double B[], int n)
{
int i, j, k;
double pivot;
double bi, elm;
for (i=1; i<=n-1; i++) {
pivot = -A[i-1][i-1];
bi = B[i-1];
for (j=i+1; j<= n; j++) {
elm = A[j-1][i-1];
A[0][j-1] = elm / pivot;
if (elm == 0) continue;
for (k=i+1; k<=j; k++)
A[j-1][k-1] += elm*A[0][k-1];
B[j-1] = B[j-1] + bi*A[0][j-1];
}
}
if (A[n-1][n-1] == 0) {
cerr << "CRingFitter::symmax()" << endl;
cerr << "\tERROR" << endl;
cerr << "\tSingular matrix on input. No further action." << endl;
return False;
}
B[n-1] = B[n-1]/A[n-1][n-1];
i = n-1;
int i1 = n;
while (i != 0) {
bi = -B[i-1];
for (j=i1; j<=n; j++)
bi = bi + A[j-1][i-1]*B[j-1];
B[i-1] = -bi/A[i-1][i-1];
i1 = i;
i--;
}
return True;
}