// vector _y as user input public static void ComputePCATransf(List <Point3D> _points, Vector3D v_correction, out Matrix3D trWC2LC, out Matrix3D trLC2WC) { trWC2LC = Matrix3D.Identity; trLC2WC = Matrix3D.Identity; if (_points == null || _points.Count < 1 || v_correction == null) { return; } Point3D pivot; Vector3D vec0, vec1, vec2; GeometryTransf.CalculatePCA(_points, out pivot, out vec0, out vec1, out vec2); // detect which base vector is closest to the correction vector v_correction.Normalize(); double angle0 = Vector3D.DotProduct(v_correction, vec0); double angle1 = Vector3D.DotProduct(v_correction, vec1); double angle2 = Vector3D.DotProduct(v_correction, vec2); Vector3D vec_to_rotate = vec2; if (Math.Abs(angle0) < Math.Abs(angle1) && Math.Abs(angle0) < Math.Abs(angle2)) { vec_to_rotate = (angle0 > 0) ? vec0 : -vec0; } else if (Math.Abs(angle1) < Math.Abs(angle0) && Math.Abs(angle1) < Math.Abs(angle2)) { vec_to_rotate = (angle1 > 0) ? vec1 : -vec1; } else { vec_to_rotate = (angle2 > 0) ? vec2 : -vec2; } // correct by user-defined vector Matrix3D R = GeometryTransf.AlingVector(v_correction, vec_to_rotate); Vector3D vec0_r = R.Transform(vec0); Vector3D vec1_r = R.Transform(vec1); Vector3D vec2_r = R.Transform(vec2); // debug List <Vector3D> ucs_1 = new List <Vector3D> { vec0, vec1, vec2, v_correction }; List <Vector3D> ucs_2 = new List <Vector3D> { vec0_r, vec1_r, vec2_r }; string ucs_1_str = GeometryTransf.VectorListToString(ucs_1); string ucs_2_str = GeometryTransf.VectorListToString(ucs_2); // debug // calculate the transforms WITH CORRECTION trWC2LC = GeometryTransf.GetTransformWC2LC(pivot, vec0_r, vec1_r, vec2_r); trLC2WC = GeometryTransf.GetTransformLC2WC(pivot, vec0_r, vec1_r, vec2_r); }
private static void CalculatePCA(List <Point3D> _points, out Point3D pivotO, out Vector3D vecX, out Vector3D vecY, out Vector3D vecZ) { pivotO = new Point3D(0, 0, 0); vecX = new Vector3D(0, 0, 0); vecY = new Vector3D(0, 0, 0); vecZ = new Vector3D(0, 0, 0); if (_points == null || _points.Count < 1) { return; } Point3D pivot = GeometryTransf.GetPivot(_points); pivotO = new Point3D(pivot.X, pivot.Y, pivot.Z); List <Vector3D> point_deviations = _points.Select(x => x - pivot).ToList(); int nrP = _points.Count; #region COVARIANCE:Old //// compute the covariance matrix //double[] m = new double[3*nrP]; //for(int i = 0; i < nrP; i++) //{ // m[i*3] = point_deviations[i].X; // m[i*3 + 1] = point_deviations[i].Y; // m[i*3 + 2] = point_deviations[i].Z; //} //MatrixNxN M = new MatrixNxN(nrP, 3, m); //MatrixNxN MtxM = MatrixNxN.Squared(M); //MtxM.Scale(1.0 / nrP); #endregion // compute the covariance matrix ... // using 3rd party library DotNetMatrix double[][] pd_as_array = new double[nrP][]; for (int i = 0; i < nrP; i++) { pd_as_array[i] = new double[] { point_deviations[i].X, point_deviations[i].Y, point_deviations[i].Z }; } GeneralMatrix gm_M = new GeneralMatrix(pd_as_array); GeneralMatrix gm_Mt = gm_M.Transpose(); GeneralMatrix gm_Msq = gm_Mt.Multiply(gm_M); GeneralMatrix gm_Msqn = gm_Msq.Multiply(1.0 / nrP); // extract the sorted Eigenvalues of the matrix... // using 3rd party library DotNetMatrix EigenvalueDecomposition decomp = gm_Msqn.Eigen(); GeneralMatrix gm_EVec = decomp.GetV(); double[] gm_EVal = decomp.RealEigenvalues; // from smallest to largest eigenvalue vecX = new Vector3D(gm_EVec.GetElement(0, 0), gm_EVec.GetElement(1, 0), gm_EVec.GetElement(2, 0)); vecY = new Vector3D(gm_EVec.GetElement(0, 1), gm_EVec.GetElement(1, 1), gm_EVec.GetElement(2, 1)); vecZ = new Vector3D(gm_EVec.GetElement(0, 2), gm_EVec.GetElement(1, 2), gm_EVec.GetElement(2, 2)); }
public static void ComputePCATransf(List <Point3D> _points, out Matrix3D trWC2LC, out Matrix3D trLC2WC) { trWC2LC = Matrix3D.Identity; trLC2WC = Matrix3D.Identity; if (_points == null || _points.Count < 1) { return; } Point3D pivot; Vector3D vec0, vec1, vec2; GeometryTransf.CalculatePCA(_points, out pivot, out vec0, out vec1, out vec2); // calculate the transforms trWC2LC = GeometryTransf.GetTransformWC2LC(pivot, vec0, vec1, vec2); trLC2WC = GeometryTransf.GetTransformLC2WC(pivot, vec0, vec1, vec2); }