LeastSquaresFitting::InitialGuessForLeastSquaresFitting LeastSquaresFitting::findInitialGuess(const Handle_TColgp_HArray1OfPnt& points)
{
//Compute center of gravity for point set
double cx=0.0;
double cy=0.0;
for(Standard_Integer i=points->Lower();i<=points->Upper();i++)
{
cx += points->Value(i).X();
cy += points->Value(i).Y();
}
cx /= points->Length();
cy /= points->Length();
gp_Pnt centerPoint(cx,cy,0.0);
//Compute average radius
double averageRadius = 0.0;
for(Standard_Integer i=points->Lower();i<=points->Upper();i++)
{
averageRadius += centerPoint.Distance(points->Value(i));
}
averageRadius /= points->Length();
return InitialGuessForLeastSquaresFitting(centerPoint,averageRadius);
}
Standard_Boolean CircleFitCostFunction::Value(const math_Vector& X, Standard_Real& F)
{
double sum = 0.0;
for(Standard_Integer i = myPointsToFitOnto->Lower();i<=myPointsToFitOnto->Upper();i++)
{
double pxMinusCx = myPointsToFitOnto->Value(i).X()-X(1);
double pyMinusCy = myPointsToFitOnto->Value(i).Y()-X(2);
sum += (pxMinusCx*pxMinusCx+pyMinusCy*pyMinusCy-X(3)*X(3))*(pxMinusCx*pxMinusCx+pyMinusCy*pyMinusCy-X(3)*X(3));
}
F = sum;
return Standard_True;
}
Standard_Boolean CircleFitCostFunction::Gradient(const math_Vector& X, math_Vector& G)
{
double g0 = 0.0;
double g1 = 0.0;
double g2 = 0.0;
for(Standard_Integer i = myPointsToFitOnto->Lower();i<=myPointsToFitOnto->Upper();i++)
{
double pxMinusCx = myPointsToFitOnto->Value(i).X()-X(1);
double pyMinusCy = myPointsToFitOnto->Value(i).Y()-X(2);
double distance = pxMinusCx*pxMinusCx+pyMinusCy*pyMinusCy-X(3)*X(3);
g0 += pxMinusCx*distance;
g1 += pyMinusCy*distance;
g2 += distance;
}
G(1) = -4.0*g0;
G(2) = -4.0*g1;
G(3) = -4.0*X(3)*g2;
return Standard_True;
}