public Sphere3D FitSphereToPoints(Collection<Point3D> points) { if (points == null) { throw new NullReferenceException(); } if (points.Count < 4) { throw new MatrixException("Need at least 4 points to fit sphere"); } var sphere3D = new Sphere3D(); var leastSquaresFit3D = new LeastSquaresFit3D(); sphere3D.Origin = leastSquaresFit3D.Centroid(points); sphere3D.Radius = leastSquaresFit3D.RmsError; var matrix = new Matrix(points.Count, 4); var vector = new Vector(points.Count); for (var i = 0; i < 50; i++) { var num = 0; foreach (var current in points) { var vector3D = Project3D.PointOntoSphere(sphere3D, current) - sphere3D.Origin; vector3D.Normalise(); matrix[num, 0] = vector3D.X; matrix[num, 1] = vector3D.Y; matrix[num, 2] = vector3D.Z; matrix[num, 3] = -1.0; var value = Distance3D.PointToSphere(sphere3D, current); vector[num] = value; num++; } var vector2 = matrix.PseudoInverse()*vector; if (vector2.Length() < 1E-06) { break; } var origin = sphere3D.Origin; origin.X += vector2[0]; var origin2 = sphere3D.Origin; origin2.Y += vector2[1]; var origin3 = sphere3D.Origin; origin3.Z += vector2[2]; sphere3D.Radius -= vector2[3]; } CalculateErrors(points, sphere3D); return sphere3D; }
public Circle3D FitCircleToPoints(Collection<Point3D> points) { if (points == null) { throw new MatrixNullReference(); } if (points.Count < 3) { throw new ArgumentException("Need at least 3 points to fit circle"); } _solver = new NRSolver(points.Count*3 + 1, 7); _measuredPoints = points; var leastSquaresFit3D = new LeastSquaresFit3D(); var plane3D = leastSquaresFit3D.FitPlaneToPoints(points); var initialGuess = new Circle3D { Origin = leastSquaresFit3D.Centroid(points), Normal = plane3D.Normal, Radius = leastSquaresFit3D.AverageError }; var vector = _solver.Solve(Circle3DErrorFunction, VectorFromCircle3D(initialGuess)); var circle3D = new Circle3D { Origin = new Point3D(vector[0], vector[1], vector[2]), Normal = new Vector3D(vector[3], vector[4], vector[5]), Radius = vector[6] }; CalculateErrors(points, circle3D); return circle3D; }
public Circle3D FitCircleToPoints2(Collection<Point3D> points) { if (points == null) { throw new MatrixNullReference(); } if (points.Count < 3) { throw new ArgumentException("Need at least 3 points to fit circle"); } var circle3D = new Circle3D(); var leastSquaresFit3D = new LeastSquaresFit3D(); var matrix = new Matrix(points.Count, 7); var vector = new Vector(points.Count); for (var i = 0; i < 50; i++) { circle3D.Origin = leastSquaresFit3D.Centroid(points); circle3D.Radius = leastSquaresFit3D.RmsError; var plane3D = leastSquaresFit3D.FitPlaneToPoints(points); circle3D.Normal = plane3D.Normal; var num = 0; foreach (var current in points) { var p = Project3D.PointOntoPlane(plane3D, current); var vector3D = circle3D.Origin - p; vector3D.Normalise(); matrix[num, 0] = vector3D[0]; matrix[num, 1] = vector3D[1]; matrix[num, 2] = vector3D[2]; matrix[num, 3] = plane3D.Normal.X; matrix[num, 4] = plane3D.Normal.Y; matrix[num, 5] = plane3D.Normal.Z; matrix[num, 6] = -1.0; var value = Distance3D.PointToCircle(circle3D, current); vector[num] = value; num++; } var vector2 = matrix.PseudoInverse()*vector; if (vector2.Length() < 1E-06) { break; } var origin = circle3D.Origin; origin.X += vector2[0]; var origin2 = circle3D.Origin; origin2.Y += vector2[1]; var origin3 = circle3D.Origin; origin3.Z += vector2[2]; var normal = circle3D.Normal; normal.X += vector2[3]; var normal2 = circle3D.Normal; normal2.Y += vector2[4]; var normal3 = circle3D.Normal; normal3.Z += vector2[5]; circle3D.Radius -= vector2[6]; } CalculateErrors(points, circle3D); return circle3D; }
public static Point3D Centroid(Collection<Point3D> points) { var leastSquaresFit3D = new LeastSquaresFit3D(); return leastSquaresFit3D.Centroid(points); }