/// <summary> /// Fit plane through points by linear orthogonal regression. /// </summary> /// <remarks>http://www.lsr.ei.tum.de/fileadmin/publications/K._Klasing/KlasingAlthoff-ComparisonOfSurfaceNormalEstimationMethodsForRangeSensingApplications_ICRA09.pdf</remarks> /// <param name="points">Points to fit by plane</param> /// <returns>Best-fit plane in terms of orthogonal least square regression.</returns> public static bool FitByPCA(IEnumerable <Vector> points, out Plane plane) { // Perform orth lin regression by PCA which requires the estimation // of the covariance matrix of the samples // http://en.wikipedia.org/wiki/Estimation_of_covariance_matrices plane = null; int count = 0; Vector mean = new Vector(3, 0.0); foreach (Vector p in points) { mean.AddInplace(p); count++; } if (count < 3) { return(false); } mean /= (double)count; Matrix cov = new Matrix(3, 3, 0.0); foreach (Vector p in points) { Vector v = p - mean; //code below is equivalent to //cov += v.ToColumnMatrix() * v.ToRowMatrix(); //optimized double v00 = v[0] * v[0]; double v01 = v[0] * v[1]; double v02 = v[0] * v[2]; double v11 = v[1] * v[1]; double v12 = v[1] * v[2]; double v22 = v[2] * v[2]; cov[0, 0] += v00; cov[0, 1] += v01; cov[0, 2] += v02; cov[1, 0] += v01; cov[1, 1] += v11; cov[1, 2] += v12; cov[2, 0] += v02; cov[2, 1] += v12; cov[2, 2] += v22; } // Skip normalization of matrix try { EigenvalueDecomposition decomp = cov.EigenvalueDecomposition; plane = new Plane(mean, decomp.EigenVectors.GetColumnVector(0)); return(true); } catch (IndexOutOfRangeException) { return(false); } }
/// <summary> /// Fit plane through points by linear orthogonal regression. /// </summary> /// <remarks>http://www.lsr.ei.tum.de/fileadmin/publications/K._Klasing/KlasingAlthoff-ComparisonOfSurfaceNormalEstimationMethodsForRangeSensingApplications_ICRA09.pdf</remarks> /// <param name="points">Points to fit by plane</param> /// <returns>Best-fit plane in terms of orthogonal least square regression.</returns> public static bool FitByPCA(IEnumerable<Vector> points, out Plane plane) { // Perform orth lin regression by PCA which requires the estimation // of the covariance matrix of the samples // http://en.wikipedia.org/wiki/Estimation_of_covariance_matrices plane = null; int count = 0; Vector mean = new Vector(3, 0.0); foreach (Vector p in points) { mean.AddInplace(p); count++; } if (count < 3) return false; mean /= (double)count; Matrix cov = new Matrix(3, 3, 0.0); foreach (Vector p in points) { Vector v = p - mean; //code below is equivalent to //cov += v.ToColumnMatrix() * v.ToRowMatrix(); //optimized double v00 = v[0] * v[0]; double v01 = v[0] * v[1]; double v02 = v[0] * v[2]; double v11 = v[1] * v[1]; double v12 = v[1] * v[2]; double v22 = v[2] * v[2]; cov[0, 0] += v00; cov[0, 1] += v01; cov[0, 2] += v02; cov[1, 0] += v01; cov[1, 1] += v11; cov[1, 2] += v12; cov[2, 0] += v02; cov[2, 1] += v12; cov[2, 2] += v22; } // Skip normalization of matrix try { EigenvalueDecomposition decomp = cov.EigenvalueDecomposition; plane = new Plane(mean, decomp.EigenVectors.GetColumnVector(0)); return true; } catch (IndexOutOfRangeException) { return false; } }