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