internal static double Fit(IEnumerable <GeoPoint2D> points, ref GeoPoint2D center2d, ref double radius)
{
GeoPoint2D[] pnts = null;
if (points is GeoPoint2D[] a)
{
pnts = a;
}
else if (points is List <GeoPoint2D> l)
{
pnts = l.ToArray();
}
else
{
List <GeoPoint2D> lp = new List <GeoPoint2D>();
foreach (GeoPoint2D point2D in points)
{
lp.Add(point2D);
}
pnts = lp.ToArray();
}
Vector <double> observedX = new DenseVector(pnts.Length); // there is no need to set values
Vector <double> observedY = new DenseVector(pnts.Length); // this is the data we want to achieve, namely 0.0
LevenbergMarquardtMinimizer lm = new LevenbergMarquardtMinimizer(gradientTolerance: 1e-12, maximumIterations: 20);
IObjectiveModel iom = ObjectiveFunction.NonlinearModel(
new Func <Vector <double>, Vector <double>, Vector <double> >(delegate(Vector <double> vd, Vector <double> ox) // function
{
// parameters: 0:cx, 1:cy, 3: radius
GeoPoint2D cnt = new GeoPoint2D(vd[0], vd[1]);
DenseVector res = new DenseVector(pnts.Length);
for (int i = 0; i < pnts.Length; i++)
{
res[i] = (pnts[i] | cnt) - vd[2];
}
#if DEBUG
double err = 0.0;
for (int i = 0; i < pnts.Length; i++)
{
err += res[i] * res[i];
}
#endif
return(res);
}),
new Func <Vector <double>, Vector <double>, Matrix <double> >(delegate(Vector <double> vd, Vector <double> ox) // derivatives
{
// parameters: 0:cx, 1:cy, 3: radius
GeoPoint2D cnt = new GeoPoint2D(vd[0], vd[1]);
var prime = new DenseMatrix(pnts.Length, 3);
for (int i = 0; i < pnts.Length; i++)
{
double d = pnts[i] | cnt;
prime[i, 0] = -(pnts[i].x - vd[0]) / d;
prime[i, 1] = -(pnts[i].y - vd[1]) / d;
prime[i, 2] = -1;
}
return(prime);
}), observedX, observedY);
NonlinearMinimizationResult mres = lm.FindMinimum(iom, new DenseVector(new double[] { center2d.x, center2d.y, radius }));
if (mres.ReasonForExit == ExitCondition.Converged || mres.ReasonForExit == ExitCondition.RelativeGradient)
{
center2d = new GeoPoint2D(mres.MinimizingPoint[0], mres.MinimizingPoint[1]);
radius = mres.MinimizingPoint[2];
double err = 0.0;
for (int i = 0; i < pnts.Length; i++)
{
err += Math.Abs((pnts[i] | center2d) - radius);
}
return(err);
}
else
{
return(double.MaxValue);
}
}
public static Ellipse2D FromPoints(IEnumerable <GeoPoint2D> points)
{
GeoPoint2D[] pnts = null;
if (points is GeoPoint2D[] a)
{
pnts = a;
}
else if (points is List <GeoPoint2D> l)
{
pnts = l.ToArray();
}
else
{
List <GeoPoint2D> lp = new List <GeoPoint2D>();
foreach (GeoPoint2D point2D in points)
{
lp.Add(point2D);
}
pnts = lp.ToArray();
}
Vector <double> observedX = new DenseVector(pnts.Length + 1); // there is no need to set values
Vector <double> observedY = new DenseVector(pnts.Length + 1); // this is the data we want to achieve, namely 1.0, points on the unit circle distance to origin
for (int i = 0; i < observedY.Count; i++)
{
observedY[i] = 1;
}
observedY[pnts.Length] = 0; // the scalar product of minor and major axis
LevenbergMarquardtMinimizer lm = new LevenbergMarquardtMinimizer(gradientTolerance: 1e-12, maximumIterations: 20);
IObjectiveModel iom = ObjectiveFunction.NonlinearModel(
new Func <Vector <double>, Vector <double>, Vector <double> >(delegate(Vector <double> vd, Vector <double> ox) // function
{
// parameters: the homogeneous matrix that projects the ellipse to the unit circle
ModOp2D m = new ModOp2D(vd[0], vd[1], vd[2], vd[3], vd[4], vd[5]);
DenseVector res = new DenseVector(pnts.Length + 1);
for (int i = 0; i < pnts.Length; i++)
{
GeoPoint2D pp = m * pnts[i];
res[i] = pp.x * pp.x + pp.y * pp.y;
}
res[pnts.Length] = m[0, 0] * m[0, 1] + m[1, 0] * m[1, 1]; // the axis should be perpendicular
#if DEBUG
double err = 0.0;
for (int i = 0; i < pnts.Length; i++)
{
err += (res[i] - 1) * (res[i] - 1);
}
err += res[pnts.Length] * res[pnts.Length];
#endif
return(res);
}),
new Func <Vector <double>, Vector <double>, Matrix <double> >(delegate(Vector <double> vd, Vector <double> ox) // derivatives
{
// parameters: the homogeneous matrix that projects the ellipse to the unit circle
ModOp2D m = new ModOp2D(vd[0], vd[1], vd[2], vd[3], vd[4], vd[5]);
var prime = new DenseMatrix(pnts.Length + 1, 6);
for (int i = 0; i < pnts.Length; i++)
{
GeoPoint2D pp = m * pnts[i];
prime[i, 0] = 2 * pnts[i].x * pp.x;
prime[i, 1] = 2 * pnts[i].y * pp.x;
prime[i, 2] = 2 * pp.x;
prime[i, 3] = 2 * pnts[i].x * pp.y;
prime[i, 4] = 2 * pnts[i].y * pp.y;
prime[i, 5] = 2 * pp.y;
}
prime[pnts.Length, 0] = m[0, 1];
prime[pnts.Length, 1] = m[0, 0];
prime[pnts.Length, 2] = 0;
prime[pnts.Length, 3] = m[1, 1];
prime[pnts.Length, 4] = m[1, 0];
prime[pnts.Length, 5] = 0;
return(prime);
}), observedX, observedY);
BoundingRect ext = new BoundingRect(pnts);
NonlinearMinimizationResult mres = lm.FindMinimum(iom, new DenseVector(new double[] { 2.0 / ext.Width, 0, -ext.GetCenter().x, 0, 2.0 / ext.Height, -ext.GetCenter().y }));
if (mres.ReasonForExit == ExitCondition.Converged || mres.ReasonForExit == ExitCondition.RelativeGradient)
{
Vector <double> vd = mres.MinimizingPoint;
ModOp2D m = new ModOp2D(vd[0], vd[1], vd[2], vd[3], vd[4], vd[5]);
m = m.GetInverse();
return(new Ellipse2D(m * GeoPoint2D.Origin, m * GeoVector2D.XAxis, m * GeoVector2D.YAxis));
}
else
{
return(null);
}
}
