circlefit.cpp

barcode readers [ from Image]

//
// Fit a circle to a collection of points using the "Iterated
// Weighted Inversion Method" described in
// "Classical geometrical approach to circle fitting --
//  review and new developments", C. Rusu, M. Tico,
// P. Kuosmanen, E. Delp, Journal of Electronic Imaging,
// 12 (1) 179-193 (1/2003). Also on web at 
// http://www.nokia.com/downloads/aboutnokia/research/library/audio_visual/AVC10.pdf
//
// Copyright (C) 2003, 2006 by Jon A. Webb (Contact via GMail; username is jonawebb)
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// 
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//
//

#include "CircleFit.h"

namespace Algorithm
{

    EXPORT_C CCircleFit* CCircleFit::NewL(TReal rEps)
    {
        return new (ELeave) CCircleFit(rEps);
    }

    CCircleFit::CCircleFit(TReal rEps) :
        _rEpsSq(rEps*rEps)
    {
    }

    CCircleFit::~CCircleFit(void)
    {
    }

    void FreeMemory(TReal* pU, TReal* pV, TReal* pW) {
		CleanupStack::Check(pW);
        CleanupStack::PopAndDestroy(); // pW
 		CleanupStack::Check(pV);
        CleanupStack::PopAndDestroy(); // pV
		CleanupStack::Check(pU);
        CleanupStack::PopAndDestroy(); // pU  
    }

    bool CCircleFit::FitCircleL(TPoint* pPoints, int nPoints, TReal& rCX, TReal& rCY, TReal& rR)
    {
        if (nPoints<3) {
            return false;
        }
        // first allocate the inversion coordinates
        TReal* pU = new (ELeave) TReal [nPoints];
        CleanupStack::PushL(pU);
        TReal* pV = new (ELeave) TReal [nPoints];
        CleanupStack::PushL(pV);
        // allocate weight vector 
        TReal* pW = new (ELeave) TReal [nPoints];
        CleanupStack::PushL(pW);

        TReal rErrSq = 0.0;
        // compute initial guess at a point on the circle for inversion (i.e., the inversion pole).
        // we want this to be as far as possible away from the other points
        // we choose a point directly opposite the middle point in the sequence
        // on the circle, assuming the estimate of the center is correct
        TReal rRhoSq = 2500.0; // this should be on the order of the square of the expected circle radius
        TReal rX = 2.0*rCX - TReal(pPoints[nPoints/2].iX);
        TReal rY = 2.0*rCY - TReal(pPoints[nPoints/2].iY);
        TReal rXK1 = 0.0, rYK1 = 0.0; // initialized by first 2 passes through the loop below
        int k = 0;
        do {
            if (k>25) {
                // the circle fit is not converging; give up
                FreeMemory(pU, pV, pW);
                return false;
            }
            int i = 0;
            TReal rUAvg = 0.0, rVAvg = 0.0, rWSum = 0.0;
            for (; i<nPoints; i++) {
                // compute the inversion coordinates
                TReal rXDiff = TReal(pPoints[i].iX) - rX;
                TReal rYDiff = TReal(pPoints[i].iY) - rY;
                TReal rDistSq = rXDiff * rXDiff + rYDiff * rYDiff;
                if (rDistSq == 0.0) {
                    // probably indicates an error in the circle fit. give up
                    FreeMemory(pU, pV, pW);
                    return false;
                }
                pU[i] = rX + (rXDiff * rRhoSq) / rDistSq;
                pV[i] = rY + (rYDiff * rRhoSq) / rDistSq;
                // compute the weights
                pW[i] = rDistSq * rDistSq;
                // sum the inversion coordinates to form average
                rUAvg += pU[i] * pW[i];
                rVAvg += pV[i] * pW[i];
                // sum of weights
                rWSum += pW[i];
            }
            // compute weighted average u and v
            if (rWSum == 0.0) {
                // indicates an error in the circle fit. give up.
                FreeMemory(pU, pV, pW);
                return false;
            }
            rUAvg /= rWSum;
            rVAvg /= rWSum;
            // compute the sums for fitting the line in the inversion space
            TReal rSuu = 0.0, rSuv = 0.0, rSvv = 0.0;
            for (i = 0; i<nPoints; i++) {
                TReal rUDiff = pU[i] - rUAvg;
                TReal rVDiff = pV[i] - rVAvg;
                rSuu += pW[i] * rUDiff * rUDiff;
                rSuv += pW[i] * rUDiff * rVDiff;
                rSvv += pW[i] * rVDiff * rVDiff;
            }
            TReal rSDiff = rSuu - rSvv;
            TReal rSqrt;
            TReal rSurd = rSDiff * rSDiff + 4 * rSvv * rSvv;
            if (rSurd < 0.0) {
                // an error in the circle fit. give up
                FreeMemory(pU, pV, pW);
                return false;
            }
            Math::Sqrt(rSqrt, rSurd);
            // compute slope (A) and v intercept (B) of line v = a + bu
            if (rSvv == 0.0) {
                // an error in the circle fit. give up
                FreeMemory(pU, pV, pW);
                return false;
            }
            TReal rB = (-rSDiff + rSqrt) / (2.0 * rSvv);
            TReal rA = rVAvg - rB * rUAvg;
            // compute nearest point on this line to the current inversion pole (rX, rY)
            TReal rXNear = (rB * rY + rX - rA * rB) / (1 + rB*rB);
            TReal rYNear = rA + rB * (rB * rY + rX - rA * rB)/ (1 + rB * rB);
            TReal rXNearDiff = rXNear - rX;
            TReal rYNearDiff = rYNear - rY;
            TReal rNearDist = rXNearDiff * rXNearDiff + rYNearDiff * rYNearDiff;
            // compute image of this nearest point on the circle. this is also the point on the
            // circle that is diametrically opposite the current inversion pole
            if (rNearDist == 0.0) {
                // an error in the circle fit. give up
                FreeMemory(pU, pV, pW);
                return false;
            }
            TReal rXImage = rX + rXNearDiff*rRhoSq / rNearDist;
            TReal rYImage = rY + rYNearDiff*rRhoSq / rNearDist;
            // compute center by averaging these two points
            rCX = (rX + rXImage) / 2;
            rCY = (rY + rYImage) / 2;
            TReal rXCXDiff = rX - rCX;
            TReal rYCYDiff = rY - rCY;
            // compute radius
            Math::Sqrt(rR, rXCXDiff * rXCXDiff + rYCYDiff * rYCYDiff);
            // stopping criterion--difference between new inversion pole and the one two
            // steps ago
            TReal rXK1Diff = rXImage - rXK1;
            TReal rYK1Diff = rYImage - rYK1;
            rErrSq = rXK1Diff * rXK1Diff + rYK1Diff * rYK1Diff;
            // update k-1 and k records of inversion pole
            rXK1 = rX;
            rYK1 = rY;
            rX = rXImage;
            rY = rYImage;
        } while (k++<=2 || // always do loop at least 3 times to initialize rErrSq properly
                 rErrSq > _rEpsSq); 
        FreeMemory(pU, pV, pW);
        return true;
    }

};