source: JEM-EUSO/esaf_cc_at_lal/packages/reconstruction/modules/shower/fitting/src/MedianFit.cc @ 114

// ESAF : Euso Simulation and Analysis Framework
// $Id: MedianFit.cc 2949 2011-06-26 18:39:34Z mabl $
// R. Pesce created Jan, 29 2004

/*****************************************************************************
 * ESAF: Euso Simulation and Analysis Framework                              *
 *                                                                           *
 *  Id: MedianFit                                                            *
 *  Package: Fitting                                                         *
 *  Coordinator: <coordinator>                                               *
 *                                                                           *
 *****************************************************************************/

//_____________________________________________________________________________
//
// TrackDirection2Module
//
// <extensive class description>

#include <iostream>
#include "MedianFit.hh"
#include "TMath.h"

using namespace TMath;

ClassImp(MedianFit)

//___________________________________________________________________________
MedianFit::MedianFit( Int_t numpoints, vector<Double_t> x, vector<Double_t> y ) {
    //
    // ctor
    //
    
    fNumPoints = numpoints;
    fSlope = 0;
    fOffset = 0;
    fAbsoluteDeviation = 0;
    fPointsX = x;
    fPointsY = y;
    Do();
}

//___________________________________________________________________________
MedianFit::~MedianFit() {
    //
    // dtor
    //
    
    fPointsX.clear();
    fPointsY.clear();
}


//___________________________________________________________________________
void MedianFit::Do() {
    //
    // Median fit algorithm
    // Adapted from "Numerical Recipes in C" pag.704
    //
   
    if ( fNumPoints < 2 ) return;
   
    Double_t XSum = 0;
    Double_t XSqSum = 0;
    Double_t YSum = 0;
    Double_t XYSum = 0;

    for( Int_t i=0; i<fNumPoints; i++ ) {
        XSum += fPointsX[i];
        XSqSum += fPointsX[i] * fPointsX[i];
        YSum += fPointsY[i];
        XYSum += fPointsX[i] * fPointsY[i];
    }

    // method of least squares for a first guess for slope and offset
    fOffset = (XSqSum * YSum - XSum * XYSum) / (fNumPoints * XSqSum - XSum * XSum);
    fSlope = (fNumPoints * XYSum - XSum * YSum) / (fNumPoints * XSqSum - XSum * XSum);
    Double_t chisquare = 0;
    for( Int_t i=0; i<fNumPoints; i++ ) {
        chisquare += Power((fPointsY[i] - (fOffset + fSlope * fPointsX[i] )),2);
    }

    // standard deviation: idea of how big an interaction step to take
    Double_t sigmaslope = Sqrt(chisquare / fNumPoints * XSqSum - XSum * XSum);
    
    Double_t b1 = fSlope;
    Double_t r1 = RoFunc( b1 );

    if ( sigmaslope > 0 ) {
        Double_t b2 = fSlope + Sign(3. * sigmaslope, r1);
        // gues bracket as 3-sigma away, in the downhill direction known from r1
        Double_t r2 = RoFunc( b2 );
        if ( b2 == b1 ) {
            fAbsoluteDeviation /= fNumPoints;
            return;
        }
        // bracketing
        while( r1*r2 > 0 ) {
            fSlope = b2 + 1.6 * ( b2 - b1 );
            b1 = b2;
            r1 = r2;
            b2 = fSlope;
            r2 = RoFunc( b2 );
        }
        sigmaslope *= 0.01;
        // refine until error a negligible number of sigmas
        while( Abs(b2-b1) > sigmaslope ) {
            fSlope = b1 + 0.5 * ( b2 - b1 );  // bisection
            if ( ( fSlope == b1 ) || ( fSlope == b2 ) ) break;
            Double_t r = RoFunc( fSlope );
            if ( r*r1 >= 0 ) {
                r1 = r;
                b1 = fSlope;
            } else {
                r2 = r;
                b2 = fSlope;
            }
        }
    }
    fAbsoluteDeviation /= fNumPoints;
}

//___________________________________________________________________________
Double_t MedianFit::RoFunc( Double_t b ) {
    //
    // Method used by median fit
    // Adapted from "Numerical Recipes in C pag.705"
    //

    vector<Double_t> dummy;
    
    for( Int_t i=0; i<fNumPoints; i++ ) {
        dummy.push_back( fPointsY[i]- b * fPointsX[i] );
    }

    if ( ( fNumPoints & 1 ) == 0 ) {
        Int_t j = fNumPoints >> 1;
        fOffset = 0.5 * (Select( j, dummy ) + Select( j + 1, dummy ));
    } else {
        fOffset = Select( (fNumPoints+1)>>1, dummy);
    }
    
    Float_t sum = 0.;
    fAbsoluteDeviation = 0.;
    for( Int_t i=0; i<fNumPoints; i++ ) {
        Double_t d = fPointsY[i] - ( fOffset + b * fPointsX[i] );
        fAbsoluteDeviation += Abs(d);
        if ( fPointsY[i] != 0 )
            d /= fPointsY[i];
        if ( Abs(d) > 1.0e-7)
            sum += ( d >= 0 ? fPointsX[i] : -fPointsX[i] );
    }
    
    return sum;
}

//___________________________________________________________________________
Double_t MedianFit::Select( Int_t k, vector<Double_t> vec ) {
    //
    // Find the k-th smallest element in a vector; used to find the median
    // Adapted from "Numerical Recipes in C" pag.342
    //
    
    Int_t l = 0;
    Int_t ir = vec.size() - 1;

    for( ; ; ) {
        if ( ir <= l + 1 ) {                           // active partition contains 1 or 2 elements
            if ( ir == l + 1 && vec[ir] < vec[l] ) {   // active partition contains 2 elements
                Double_t temp = vec[l]; vec[l] = vec[ir]; vec[ir] = temp;
            }
            return vec[k-1];
        } else {
            // choose median of left, center and right elements as partitioning element a
            // rearrange so that vec[l] <= vec[l+1] <= vec[ir] 
            Int_t middle = ( l + ir ) >> 1;
            Double_t temp = vec[middle]; vec[middle] = vec[l+1]; vec[l+1] = temp;
            if ( vec[l] > vec[ir] ) {
                temp = vec[l]; vec[l] = vec[ir]; vec[ir] = temp;
            }
            if ( vec[l+1] > vec[ir] ) {
                temp = vec[l+1]; vec[l+1] = vec[ir]; vec[ir] = temp;
            }
            if ( vec[l] > vec[l+1] ) {
                temp = vec[l]; vec[l] = vec[l+1]; vec[l+1] = temp;
            }
            // initialize pointers for partitioning
            Int_t i = l + 1;
            Int_t j = ir;
            Double_t a = vec[l+1];        // partitioning element
            for( ; ; ) {
                do {                     // scan up to find element > a
                    i++;
                } while( vec[i] < a );   // && i< (Int_t)vec.size() );
                do {                     // scan down to find element < a
                    j--;
                } while( vec[j] > a );  //&& j>=0 );
                if ( j < i ) break;      // pointers crossed; partitioning complete
                temp = vec[i]; vec[i] = vec[j]; vec[j] = temp;
            }
            vec[l+1] = vec[j];           // insert partitioning element
            vec[j] = a;
            if ( j >= (k-1) ) ir = j - 1;    // keep active the partiction that contains the k-th element
            if ( j <= (k-1) ) l = i;
        }
    }
}