public static LineParams FitLine(List <PosXYZ> pos) { var points = pos.Select(p => new Vector3d(p.X, p.Y, p.Z)); var Origin = Vector3d.Zero; int num1 = 0; foreach (Vector3d point in points) { Origin += point; ++num1; } double num2 = 1.0 / num1; Origin *= num2; double num3 = 0.0; double num4 = 0.0; double num5 = 0.0; double num6 = 0.0; double num7 = 0.0; double num8 = 0.0; foreach (Vector3d point in points) { Vector3d vector3d = point - Origin; num3 += vector3d[0] * vector3d[0]; num4 += vector3d[0] * vector3d[1]; num5 += vector3d[0] * vector3d[2]; num6 += vector3d[1] * vector3d[1]; num7 += vector3d[1] * vector3d[2]; num8 += vector3d[2] * vector3d[2]; } double num9 = num3 * num2; double num10 = num4 * num2; double num11 = num5 * num2; double num12 = num6 * num2; double num13 = num7 * num2; double num14 = num8 * num2; double[] input = new double[9] { num9, num10, num11, num10, num12, num13, num11, num13, num14 }; SymmetricEigenSolver symmetricEigenSolver = new SymmetricEigenSolver(3, 4096); int num15 = symmetricEigenSolver.Solve(input, SymmetricEigenSolver.SortType.Decreasing); var ResultValid = num15 > 0 && num15 < int.MaxValue; var Normal = new Vector3d(symmetricEigenSolver.GetEigenvector(0)); return(new LineParams() { Direction = Normal, Origin = Origin }); }
public GaussPointsFit3(IEnumerable <Vector3D> points) { Box = new Box3d(Vector3D.Zero, Vector3D.One); // Compute the mean of the points. int numPoints = 0; foreach (Vector3D v in points) { Box.Center += v; numPoints++; } double invNumPoints = (1.0) / numPoints; Box.Center *= invNumPoints; // Compute the covariance matrix of the points. double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0; double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0; foreach (Vector3D p in points) { Vector3D diff = p - Box.Center; sumXX += diff[0] * diff[0]; sumXY += diff[0] * diff[1]; sumXZ += diff[0] * diff[2]; sumYY += diff[1] * diff[1]; sumYZ += diff[1] * diff[2]; sumZZ += diff[2] * diff[2]; } sumXX *= invNumPoints; sumXY *= invNumPoints; sumXZ *= invNumPoints; sumYY *= invNumPoints; sumYZ *= invNumPoints; sumZZ *= invNumPoints; double[] matrix = new double[] { sumXX, sumXY, sumXZ, sumXY, sumYY, sumYZ, sumXZ, sumYZ, sumZZ }; // Setup the eigensolver. SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096); int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing); ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE); Box.Extent = new Vector3D(solver.GetEigenvalues()); double[] evectors = solver.GetEigenvectors(); Box.AxisX = new Vector3D(evectors[0], evectors[1], evectors[2]); Box.AxisY = new Vector3D(evectors[3], evectors[4], evectors[5]); Box.AxisZ = new Vector3D(evectors[6], evectors[7], evectors[8]); }
public OrthogonalPlaneFit3(IEnumerable <Vector3D> points) { // Compute the mean of the points. Origin = Vector3D.Zero; int numPoints = 0; foreach (Vector3D v in points) { Origin += v; numPoints++; } double invNumPoints = (1.0) / numPoints; Origin *= invNumPoints; // Compute the covariance matrix of the points. double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0; double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0; foreach (Vector3D p in points) { Vector3D diff = p - Origin; sumXX += diff[0] * diff[0]; sumXY += diff[0] * diff[1]; sumXZ += diff[0] * diff[2]; sumYY += diff[1] * diff[1]; sumYZ += diff[1] * diff[2]; sumZZ += diff[2] * diff[2]; } sumXX *= invNumPoints; sumXY *= invNumPoints; sumXZ *= invNumPoints; sumYY *= invNumPoints; sumYZ *= invNumPoints; sumZZ *= invNumPoints; double[] matrix = new double[] { sumXX, sumXY, sumXZ, sumXY, sumYY, sumYZ, sumXZ, sumYZ, sumZZ }; // Setup the eigensolver. SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096); int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Decreasing); ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE); Normal = new Vector3D(solver.GetEigenvector(2)); }