public static PolynomialData LsPolynomial(Vector <double> data, Vector <double> index, int order) { if (data == null) { throw new ArgumentNullException("data"); } if (index == null) { throw new ArgumentNullException("index"); } if (order != 3 && order != 5) { throw new NotImplementedException("Uniquement le 3e et le 5e ordre sont implantés"); } var dataOut = new PolynomialData(order); Matrix <double> V = MatrixOperation.VandermondeMatrix(index, order); Vector <double> p = V.QR(QRMethod.Full).Solve(data); Vector <double> r = data - (V * p); dataOut.PolynomialParams = Vector <double> .Build.DenseOfArray(p.ToArray()); dataOut.VandermondeMatrix = V; dataOut.FreedomDegrees = data.Count - (order + 1); dataOut.ResidualNorm = r.L2Norm(); dataOut.PolynomialOrder = order; return(dataOut); }
public static LineData Ls2Dline(Matrix <double> data) { if (data == null) { throw new ArgumentNullException("data"); } var dataTmp = data.Clone(); LineData dataOut = new LineData(dataTmp.RowCount); // Check number of data points if (dataTmp.RowCount < 2) { return(dataOut); } // Calculate centroid dataOut.Centroid[0] = 0; dataOut.Centroid[1] = 0; for (int i = 0; i < dataTmp.RowCount; i++) { dataOut.Centroid[0] += dataTmp[i, 0]; dataOut.Centroid[1] += dataTmp[i, 1]; } dataOut.Centroid[0] /= dataTmp.RowCount; dataOut.Centroid[1] /= dataTmp.RowCount; // Form matrix a of translated points for (int i = 0; i < dataTmp.RowCount; i++) { dataTmp[i, 0] -= dataOut.Centroid[0]; dataTmp[i, 1] -= dataOut.Centroid[1]; } var svd = dataTmp.Svd(computeVectors: true); // Find the largest singular value in S and extract from V the // corresponding right singular vector dataOut.LineParams[0] = svd.VT[0, 0]; dataOut.LineParams[1] = svd.VT[0, 1]; // Calculate residual distances for (int i = 0; i < dataTmp.RowCount; i++) { var vDiff = Vector <double> .Build.Dense(new[] { dataTmp[i, 0], dataTmp[i, 1] }); dataOut.Residuals[i] = MatrixOperation.CrossProduct(vDiff, dataOut.LineParams).L2Norm(); dataOut.ResidualsNorm += Math.Pow(dataOut.Residuals[i], 2); } dataOut.ResidualsNorm = Math.Sqrt(dataOut.ResidualsNorm); // Calculate intersection point var startPoint = Vector <double> .Build.Dense(new[] { data[0, 0], data[0, 1] }); var endPoint = Vector <double> .Build.Dense(new[] { data[data.RowCount - 1, 0], data[data.RowCount - 1, 1] }); var intersectionPoint = GetIntersectionPoint(startPoint, endPoint, dataOut.LineParams, dataOut.Centroid); if (intersectionPoint == null) { //TODO: JFF: À revoir... Ajouté parce qu'il arrivait que lfU soit non numérique et que si les intersections restaient à 0 alors d'autres erreurs arrivaient plus loin. Pour l'instant c'est la correction la plus simple mais lorsqu'on aura un peu plus de temps, se pencher d'avantage sur une correction/gestion plus viable de cette erreur. Erreur qui arrivait par exemple lorsque la matrix d'entré contenait 2 coordonnées et que ses dernières étaient exactement les mêmes. dataOut.IntersectionPoint[0] = data[data.RowCount - 1, 0]; dataOut.IntersectionPoint[1] = data[data.RowCount - 1, 1]; } return(dataOut); }
public static Circle3DData Ls3DCircle(Matrix <double> data, int startPointIndex) { if (data == null) { throw new ArgumentNullException("data"); } var dataTmp = data.Clone(); var dataOut = new Circle3DData(); // Check number of data points if (dataTmp.RowCount < 2) { return(dataOut); } PlaneData planeData = LsPlane(dataTmp); dataOut.PlaneCentroid = planeData.Centroid; // Form matrix A of translated points for (int i = 0; i < dataTmp.RowCount; i++) { dataTmp[i, 0] -= dataOut.PlaneCentroid[0]; dataTmp[i, 1] -= dataOut.PlaneCentroid[1]; dataTmp[i, 2] -= dataOut.PlaneCentroid[2]; } // Transform the data to close to standard position via a rotation // followed by a translation dataOut.RotationMatrix = MatrixOperation.RotationZ_3D(planeData.PlaneParams); dataOut.RotationCentroid = dataOut.RotationMatrix * dataOut.PlaneCentroid; Matrix <double> rotatedData = (dataOut.RotationMatrix * dataTmp.Transpose()).Transpose(); //Translate data to the rotated origin for (int i = 0; i < dataTmp.RowCount; i++) { rotatedData[i, 0] -= dataOut.RotationCentroid[0]; rotatedData[i, 1] -= dataOut.RotationCentroid[1]; rotatedData[i, 2] -= dataOut.RotationCentroid[2]; } CircleData circle2DData = Ls2DCircle(rotatedData); dataOut.CircleCentroid = circle2DData.CircleCentroid; dataOut.CircleRadius = circle2DData.CircleRadius; // THETA is a counterclockwise angular displacement in radians from the // positive x-axis, RHO is the distance from the origin to a point in the x-y plane for (int i = 0; i < rotatedData.RowCount; i++) { rotatedData[i, 0] -= dataOut.CircleCentroid[0]; rotatedData[i, 1] -= dataOut.CircleCentroid[1]; } //Convert start point to polar coordinate Vector <double> startPolar = GpsHelper.ConvertCartesianToPolar(rotatedData[startPointIndex, 0], rotatedData[startPointIndex, 1], rotatedData[startPointIndex, 2]); dataOut.IsAscending = GetCircleDirection(rotatedData.Row(0), rotatedData.Row(startPointIndex), startPolar[0], dataOut.CircleRadius); dataOut.Theta = startPolar[0]; dataOut.Z = startPolar[2]; return(dataOut); }
public static Line3DData Ls3Dline(Matrix <double> data, int startPointIndex) { if (data == null) { throw new ArgumentNullException("data"); } //------------------------------------------------------------- // DÉBUT Fit 3DLine //------------------------------------------------------------- var dataTmp = data.Clone(); Line3DData dataOut = new Line3DData(dataTmp.RowCount); // Check number of data points if (dataTmp.RowCount < 2) { return(dataOut); } // Calculate centroid dataOut.Centroid[0] = 0; dataOut.Centroid[1] = 0; dataOut.Centroid[2] = 0; for (int i = 0; i < dataTmp.RowCount; i++) { dataOut.Centroid[0] += dataTmp[i, 0]; dataOut.Centroid[1] += dataTmp[i, 1]; dataOut.Centroid[2] += dataTmp[i, 2]; } dataOut.Centroid[0] /= dataTmp.RowCount; dataOut.Centroid[1] /= dataTmp.RowCount; dataOut.Centroid[2] /= dataTmp.RowCount; // Form matrix a of translated points for (int i = 0; i < dataTmp.RowCount; i++) { dataTmp[i, 0] -= dataOut.Centroid[0]; dataTmp[i, 1] -= dataOut.Centroid[1]; dataTmp[i, 2] -= dataOut.Centroid[2]; } var svd = dataTmp.Svd(computeVectors: true); // Find the largest singular value in S and extract from V the // corresponding right singular vector dataOut.LineParams[0] = svd.VT[0, 0]; dataOut.LineParams[1] = svd.VT[0, 1]; dataOut.LineParams[2] = svd.VT[0, 2]; // Calculate residual distances for (int i = 0; i < dataTmp.RowCount; i++) { var vDiff = Vector <double> .Build.Dense(new[] { dataTmp[i, 0], dataTmp[i, 1], dataTmp[i, 2] }); dataOut.Residuals[i] = MatrixOperation.CrossProduct(vDiff, dataOut.LineParams).L2Norm(); } dataOut.ResidualsNorm = dataOut.Residuals.L2Norm(); //------------------------------------------------------------- // FIN Fit 3DLine //------------------------------------------------------------- //TODO: Michel Robert: Il faut refactoriser, il s'agit de 2 concepts, donc 2 fonctions //------------------------------------------------------------- // Début Extrapolation //------------------------------------------------------------- // Calculate intersection point var startPoint = Vector <double> .Build.Dense(new[] { data[0, 0], data[0, 1], data[0, 2] }); var endPoint = Vector <double> .Build.Dense(new[] { data[startPointIndex, 0], data[startPointIndex, 1], data[startPointIndex, 2] }); var intersectionPoint = GetIntersectionPoint(startPoint, endPoint, dataOut.LineParams, dataOut.Centroid); if (intersectionPoint == null) { //TODO: JFF: À revoir... Ajouté parce qu'il arrivait que lfU soit non numérique et que si // les intersections restaient à 0 alors d'autres erreurs arrivaient plus loin. // Pour l'instant c'est la correction la plus simple mais lorsqu'on aura un peu plus de temps, // se pencher d'avantage sur une correction/gestion plus viable de cette erreur. // Erreur qui arrivait par exemple lorsque la matrix d'entrée contenait 2 coordonnées et que // ces dernières étaient exactement les mêmes. dataOut.IntersectionPoint = endPoint; } else { dataOut.IntersectionPoint = intersectionPoint; } if (!GetLineDirection(startPoint, dataOut.IntersectionPoint, dataOut.LineParams)) { dataOut.LineParams = dataOut.LineParams * -1; } return(dataOut); }