Function fitter VBA – Conjugate Gradient

I found a nice post about applying conjugate gradient for a function fitter. They show how to use the conjugate gradient with POLAK and RIBIÈRE factor to fit a circle to a set of points. The PDF is very explicative, so I learnt a lot about the mathematics behaind this. Now it will be a lot easier to apply this to a clothoid.

The original code is a Java one, so here is translated to VBA. Will come very handy for an “alignment from points” procedure, although you will need to separate the different alignments before applying this.

Option Explicit

' Class fitting a circle to a set of points.
' This class implements the fitting algorithms described in the paper 
' Finding the circle that best fits a set of points
' @author Luc Maisonobe

' Copyright (c) 2005-2007, Luc Maisonobe
' All rights reserved.
'
' Redistribution and use in source and binary forms, with
' or without modification, are permitted provided that
' the following conditions are met:
'
'    Redistributions of source code must retain the
'    above copyright notice, this list of conditions and
'    the following disclaimer.
'    Redistributions in binary form must reproduce the
'    above copyright notice, this list of conditions and
'    the following disclaimer in the documentation
'    and/or other materials provided with the
'    distribution.
'    Neither the names of spaceroots.org, spaceroots.com
'    nor the names of their contributors may be used to
'    endorse or promote products derived from this
'    software without specific prior written permission.
'
' THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
' CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
' WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
' WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
' PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
' THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
' DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
' CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
' PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
' USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
' HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
' IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
' NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
' USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
' POSSIBILITY OF SUCH DAMAGE.

Public Type tXYZ
    X As Double
    Y As Double
    Z As Double
End Type

Private Type tCircleFitter
    Pt() As tXYZ
    Center As tXYZ
    Radius As Double
End Type

Private fitter As tCircleFitter
Private r´ As Double       ' optimum circle radius
Private Jacb As Double
Private dJx As Double
Private dJy As Double

Private Const PI As Double = 3.14159265358979
Private Const EPSILON As Double = 0.00000001

Private Sub sCircleFitter_Main()
    On Local Error GoTo CtrlErr

    Dim iter As Long

    ' fit a circle to the test points
    Call fPointsInitialize(fitter.Pt(), 15, 100, 100, 35)

    'fitter = New CircleFitter
    Call fitterInitialize(fitter.Pt())
'Debug.Print "initial circle: " & fitter.Center.X & " " & fitter.Center.Y & " " & fitter.Radius

    ' minimize the residuals
    iter = minimize(100, 0.1, EPSILON)
'Debug.Print "converged after " & iter & " iterations"
'Debug.Print "final circle: " & fitter.Center.X & " " & fitter.Center.Y & " " & fitter.Radius

ExitProc:
    Exit Sub

CtrlErr:
    Stop
End Sub

Public Function NewPoint(Optional ByVal X As Double = 0, _
                         Optional ByVal Y As Double = 0, _
                         Optional ByVal Z As Double = 0) As tXYZ
    With NewPoint
        .X = X
        .Y = Y
        .Z = Z
    End With
End Function

Private Function fPointsInitialize(ByRef oPt() As tXYZ, _
                                   ByVal lgPts As Long, _
                                   ByVal Xcenter As Double, _
                                   ByVal Ycenter As Double, _
                                   ByVal Radius As Double) As Boolean
' Initialize an approximate circle based on all triplets.
' @param lgPts number of sample points
    Dim lgPt As Long
    Dim dbAngleRAD As Double

    ReDim oPt(0 To lgPts - 1)
    VBA.Randomize
    For lgPt = 1 To lgPts
        dbAngleRAD = (Rnd() * 2 * PI)
        With oPt(lgPt - 1)
            .X = (Xcenter + Radius * Cos(dbAngleRAD)) + (CLng(IIf(Rnd() > 0.5, 1, -1)) * Rnd() * 0.01)
            .Y = (Ycenter + Radius * Sin(dbAngleRAD)) + (CLng(IIf(Rnd() > 0.5, 1, -1)) * Rnd() * 0.01)
        End With
    Next lgPt
End Function

Public Sub fitterInitialize(ByRef oPt() As tXYZ)
' Initialize an approximate circle based on all triplets.
' @param oPt() circular ring sample points
' @exception Error if all points are aligned
 
    Dim n As Long 'number of filtered points
    Dim lgPt1 As Long
    Dim lgPt2 As Long
    Dim lgPt3 As Long
    Dim cc As tXYZ
    Dim lgRetVal As Long

    With fitter
        With .Center
            .X = 0
            .Y = 0
        End With
        .Radius = 0
    End With

    ' analyze all possible points triplets
    n = 0
    For lgPt1 = LBound(oPt) To UBound(oPt) - 2
        For lgPt2 = lgPt1 + 1 To UBound(oPt) - 1
            For lgPt3 = lgPt2 + 1 To UBound(oPt)
                ' compute the triangle circumcenter
                ' Check points are not aligned
                If Not fAligned(oPt(lgPt1), oPt(lgPt2), oPt(lgPt3)) Then
                    cc = circumcenter(oPt(lgPt1), oPt(lgPt2), oPt(lgPt3))
                    ' the points are not aligned, we have a circumcenter
                    n = (n + 1)
                    fitter.Center.X = (fitter.Center.X + cc.X)
                    fitter.Center.Y = (fitter.Center.Y + cc.Y)
                End If
            Next lgPt3
        Next lgPt2
    Next lgPt1
    
    If (n = 0) Then
        lgRetVal = VBA.MsgBox("all points are aligned")
    Else
        ' initialize using the circumcenters average
        With fitter
            With .Center
                .X = .X / n
                .Y = .Y / n
            End With
            Call updateRadius(.Pt())
        End With
    End If
End Sub

Public Function fAligned(ByRef oPt1 As tXYZ, _
                         ByRef oPt2 As tXYZ, _
                         ByRef oPt3 As tXYZ) As Boolean
    fAligned = (((oPt2.X - oPt1.X) * (oPt3.Y - oPt1.Y)) - ((oPt2.Y - oPt1.Y) * (oPt3.X - oPt1.X)) < EPSILON)
End Function

Private Function updateRadius(ByRef oPt() As tXYZ) As Boolean
' Update the circle radius

    Dim lgPt As Long
    Dim dx As Double
    Dim dy As Double
    
    r´ = 0
    If Not (Not oPt) Then
        For lgPt = LBound(oPt) To UBound(oPt)
            dx = (oPt(lgPt).X - fitter.Center.X)
            dy = (oPt(lgPt).Y - fitter.Center.Y)
            r´ = (r´ + VBA.Sqr(((dx * dx) + (dy * dy))))
        Next lgPt
        
        r´ = r´ / (UBound(oPt) - LBound(oPt) + 1)
        fitter.Radius = r´
    End If
End Function

Private Function circumcenter(ByRef PI As tXYZ, ByRef pJ As tXYZ, ByRef pK As tXYZ) As tXYZ
' Compute the circumcenter of three points.
' @param pI first point
' @param pJ second point
' @param pK third point
' @return circumcenter of pI, pJ and pK or null if the points are aligned

    Dim dIJ As tXYZ
    Dim dJK As tXYZ
    Dim dKI As tXYZ
    Dim sqI As Double
    Dim sqJ As Double
    Dim sqK As Double
    Dim det As Double

    dIJ = NewPoint((pJ.X - PI.X), (pJ.Y - PI.Y))
    dJK = NewPoint((pK.X - pJ.X), (pK.Y - pJ.Y))
    dKI = NewPoint((PI.X - pK.X), (PI.Y - pK.Y))
    sqI = ((PI.X * PI.X) + (PI.Y * PI.Y))
    sqJ = ((pJ.X * pJ.X) + (pJ.Y * pJ.Y))
    sqK = ((pK.X * pK.X) + (pK.Y * pK.Y))

    ' determinant of the linear system: 0 for aligned points
    det = ((dJK.X * dIJ.Y) - (dIJ.X * dJK.Y))

    If (VBA.Abs(det) < EPSILON) Then Exit Function ' points are almost aligned, we cannot compute the circumcenter
    
    ' beware, there is a minus sign on Y coordinate!
    circumcenter = NewPoint(((sqI * dJK.Y) + ((sqJ * dKI.Y) + (sqK * dIJ.Y))) / (2 * det), _
                           -((sqI * dJK.X) + ((sqJ * dKI.X) + (sqK * dIJ.X))) / (2 * det))
End Function

Public Function minimize(ByVal iterMax As Integer, ByVal innerThreshold As Double, ByVal outerThreshold As Double) As Long
' Minimize the distance residuals between the points and the circle.
' We use a non-linear conjugate gradient method with the Polak and
' Ribiere coefficient for the computation of the search direction. The
' inner minimization along the search direction is performed using a
' few Newton steps. It is worthless to spend too much time on this inner
' minimization, so the convergence threshold can be rather large.
'
' @param maxIter maximal iterations number on the inner loop (cumulated across outer loop iterations)
' @param innerThreshold inner loop threshold, as a relative difference on the cost function value between the two last iterations
' @param outerThreshold outer loop threshold, as a relative difference on the cost function value between the two last iterations
' @return number of inner loop iterations performed (cumulated across outer loop iterations)
' @exception LocalException if we come accross a singularity or if we exceed the maximal number of iterations
    
    Dim lgRetVal As Long
    Dim u As Double
    Dim v As Double
    Dim beta As Double
    Dim lambda As Double
    Dim innerJ As Double
    Dim previousJ As Double: previousJ = Jacb
    Dim previousV As Double: previousV = 0
    Dim previousU As Double: previousU = 0
    Dim previousdJy As Double: previousdJy = 0
    Dim previousdJx As Double: previousdJx = 0
    Dim iterations As Integer: iterations = 0
  
    Call computeCost(fitter.Pt())

    If ((Jacb < EPSILON) Or (VBA.Sqr(((dJx * dJx) + (dJy * dJy))) < EPSILON)) Then
        ' we consider we are already at a local minimum
        minimize = 0
        Exit Function
    End If
    
    Do While (iterations < iterMax)
        ' search direction
        u = (dJx * -1)
        v = (dJy * -1)
        If (iterations <> 0) Then
            ' Polak-Ribiere coefficient
            beta = (((dJx * (dJx - previousdJx)) + (dJy * (dJy - previousdJy))) / ((previousdJx * previousdJx) + (previousdJy * previousdJy)))
            u = (u + (beta * previousU))
            v = (v + (beta * previousV))
        End If
        
        previousdJx = dJx
        previousdJy = dJy
        previousU = u
        previousV = v

        ' rough minimization along the search direction
        Do
            With fitter
                innerJ = Jacb
                lambda = newtonStep(.Pt(), u, v)
                .Center.X = (fitter.Center.X + (lambda * u))
                .Center.Y = (fitter.Center.Y + (lambda * v))
                Call updateRadius(.Pt())
                Call computeCost(.Pt())
            
                iterations = iterations + 1
            End With
        Loop While ((iterations < iterMax) And ((VBA.Abs((Jacb - innerJ)) / Jacb) > innerThreshold))
        
        If ((VBA.Abs((Jacb - previousJ)) / Jacb) < outerThreshold) Then
            minimize = iterations
            Exit Function
        End If
        
        previousJ = Jacb
    Loop
    
    lgRetVal = VBA.MsgBox("unable to converge after " & iterMax & " iterations")
End Function

Private Sub computeCost(ByRef oPt() As tXYZ)
' Compute the cost function and its gradient.
    Dim lgRetVal As Long
    Dim lgPt As Long
    Dim dx As Double
    Dim dy As Double
    Dim di As Double
    Dim dr As Double
    Dim ratio As Double

    Jacb = 0
    dJx = 0
    dJy = 0
    For lgPt = LBound(oPt) To UBound(oPt)
        dx = (oPt(lgPt).X - fitter.Center.X)
        dy = (oPt(lgPt).Y - fitter.Center.Y)
        di = VBA.Sqr(((dx * dx) + (dy * dy)))
        If (di < EPSILON) Then lgRetVal = VBA.MsgBox(("cost singularity: point at the circle center"))
        
        dr = (di - r´)
        ratio = (dr / di)
        Jacb = (Jacb + (dr * (di + r´)))
        dJx = (dJx + (dx * ratio))
        dJy = (dJy + (dy * ratio))
    Next lgPt
    
    dJx = (dJx * 2)
    dJy = (dJy * 2)
End Sub

Private Function newtonStep(ByRef oPt() As tXYZ, ByVal u As Double, ByVal v As Double) As Double
' Compute the length of the Newton step in the search direction.
' @param u abscissa of the search direction
' @param v ordinate of the search direction
' @return value of the step along the search direction

    Dim sumFac2R As Double
    Dim sum1 As Double
    Dim sum2 As Double
    Dim sumFac As Double
    Dim lgPt As Long
    Dim dx As Double
    Dim dy As Double
    Dim di As Double
    Dim coeff1 As Double
    Dim coeff2 As Double

    sumFac2R = 0
    sum1 = 0
    sum2 = 0
    sumFac = 0

    ' compute the first and second derivatives of the cost along the specified search direction
    For lgPt = LBound(oPt) To UBound(oPt)
        dx = (fitter.Center.X - oPt(lgPt).X)
        dy = (fitter.Center.Y - oPt(lgPt).Y)
        di = VBA.Sqr(((dx * dx) + (dy * dy)))
        coeff1 = (((dx * u) + (dy * v)) / di)
        coeff2 = (di - r´)
        sum1 = (sum1 + (coeff1 * coeff2))
        sum2 = (sum2 + (coeff2 / di))
        sumFac = (sumFac + coeff1)
        sumFac2R = (sumFac2R + (coeff1 * (coeff1 / di)))
    Next lgPt
    
    ' step length attempting to nullify the first derivative
    newtonStep = ((sum1 / (((((u * u) + (v * v)) * sum2) - (sumFac * (sumFac / (UBound(oPt) - LBound(oPt) + 1)))) + (r´ * sumFac2R))) * -1)
End Function

'Public Function getCenter(ByRef fitter As tCircleFitter) As Boolean
'    fitter.Center = oCenter: getCenter = True
'End Function

'Public Function getRadius(ByRef fitter As tCircleFitter) As Boolean
'    fitter.Radius = r´: getRadius = True
'End Function

Leave a Reply

Your email address will not be published. Required fields are marked *