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); } }