Ejemplo n.º 1
0
    private OnePC powerMethod(double[,] covar)
    {
        double sqr3 = 1.0f / Mathf.Sqrt(3.0f);

        double[] pc1    = { sqr3, sqr3, sqr3 };
        double   shrink = 0.0;

        for (int k = 0; k < 20; k++)
        {
            double[] pc1a = { 0.0f, 0.0f, 0.0f };
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    pc1a[i] += covar[i, j] * pc1[j];
                }
            }
            double shrink2  = pc1a[0] * pc1a[0] + pc1a[1] * pc1a[1] + pc1a[2] * pc1a[2];
            float  shrink2f = (float)shrink2;
            shrink = Mathf.Sqrt(shrink2f);
            for (int i = 0; i < 3; i++)
            {
                pc1[i] = pc1a[i] / shrink;
            }
        }
        OnePC pc = new OnePC(pc1, shrink);

        return(pc);
    }
Ejemplo n.º 2
0
    private double[] fitNearestSurface(List <DistanceAndPoint> distancesAndPoints, int n)
    {
        if (DEBUG)
        {
            Debug.Log("TangoUpdate pca , for " + n + " points");
        }
        MatrixStats mc = getCovar(distancesAndPoints, n);

        OnePC pc1 = powerMethod(mc.covar);

        if (DEBUG)
        {
            Debug.Log("TangoUpdate pca1 " + pc1.pc[0] + ", " + pc1.pc[1] + ", " + pc1.pc[2]);
        }

        double[,] covar2 = new double[3, 3];
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                covar2[i, j] = mc.covar[i, j] - (pc1.shrink * pc1.pc[i] * pc1.pc[j]);
            }
        }

        OnePC pc2 = powerMethod(covar2);

        if (DEBUG)
        {
            Debug.Log("TangoUpdate pca2 " + pc2.pc[0] + ", " + pc2.pc[1] + ", " + pc2.pc[2]);
        }

        //  c(x=y1 * z2 - z1 * y2, y=z1 * x2 - x1 * z2, z=x1 * y2 - y1 * x2)
        double[] planeCoefficients =
        {
            pc1.pc [1] * pc2.pc [2] - pc1.pc [2] * pc2.pc [1],
            pc1.pc [2] * pc2.pc [0] - pc1.pc [0] * pc2.pc [2],
            pc1.pc [0] * pc2.pc [1] - pc1.pc [1] * pc2.pc [0],
            0.0f
        };

        planeCoefficients[3] = planeCoefficients[0] * mc.mean[0] + planeCoefficients[1] * mc.mean[1] + planeCoefficients[2] * mc.mean[2];

        return(planeCoefficients);
    }
Ejemplo n.º 3
0
 private OnePC powerMethod(double[,] covar)
 {
     double sqr3 = 1.0f / Mathf.Sqrt (3.0f);
     double[] pc1 = {sqr3, sqr3, sqr3};
     double shrink = 0.0;
     for (int k = 0; k < 20; k++) {
         double[] pc1a = {0.0f, 0.0f, 0.0f};
         for (int i=0; i < 3; i++) {
             for (int j=0; j < 3; j++) {
                 pc1a[i] += covar[i,j] * pc1[j];
             }
         }
         double shrink2 = pc1a[0] * pc1a[0] + pc1a[1] * pc1a[1] + pc1a[2] * pc1a[2];
         float shrink2f = (float) shrink2;
         shrink = Mathf.Sqrt(shrink2f);
         for (int i=0; i < 3; i++) {
             pc1[i] = pc1a[i] / shrink;
         }
     }
     OnePC pc = new OnePC (pc1, shrink);
     return pc;
 }