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);
}
double calculateTriangleArea(const Handle_TColgp_HArray1OfPnt triangleCorners)
{
//We compute the area of the triangle using the determinant approach
//First, allocate a 3 by 3 matrix for the calculation
math_Matrix matrixForAreaCalculation(0,2,0,2);
//Then fill it with the coordinates of the triangle corners
matrixForAreaCalculation(0,0) = triangleCorners->Value(1).X();
matrixForAreaCalculation(0,1) = triangleCorners->Value(1).Y();
matrixForAreaCalculation(0,2) = 1.0;
matrixForAreaCalculation(1,0) = triangleCorners->Value(2).X();
matrixForAreaCalculation(1,1) = triangleCorners->Value(2).Y();
matrixForAreaCalculation(1,2) = 1.0;
matrixForAreaCalculation(2,0) = triangleCorners->Value(3).X();
matrixForAreaCalculation(2,1) = triangleCorners->Value(3).Y();
matrixForAreaCalculation(2,2) = 1.0;
double area = 0.5 * matrixForAreaCalculation.Determinant();
return area;
}
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;
}