Examples with Point3d maspack.matrix.Point3d used on opensource projects

Example 56 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class CPD method computeMean.

/**
 * Compute and return the weighted mean
 * @param pnts set of points
 * @param P vector of probabilities
 * @param Np sum of probabilities
 * @param mean mean to fill, if null, creates new
 * @return the weighted mean
 */
public static Point3d computeMean(Point3d[] pnts, double[] P, double Np, Point3d mean) {
    if (mean == null) {
        mean = new Point3d();
    }
    mean.setZero();
    for (int i = 0; i < P.length; i++) {
        mean.scaledAdd(P[i], pnts[i]);
    }
    mean.scale(1.0 / Np);
    return mean;
}

Example 57 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class CPD method computeP.

/**
 * Computes the CPD probability function P(m|n)
 * @param X Input points
 * @param TY Transformed output points
 * @param sigma2 variance
 * @param w weight to account for noise/outliers
 * @param P MxN probability matrix
 * @param P1 Mx1 vector, P*1
 * @param Pt1 Nx1 vector, trans(P)*1
 * @param tol2 squared point tolerance
 * @return Np the sum of all entries in P
 */
public static double computeP(Point3d[] X, Point3d[] TY, double sigma2, double w, double[][] P, double[] P1, double[] Pt1, double tol2) {
    double N = X.length;
    double M = TY.length;
    double Np = 0;
    double c = 2 * Math.PI * sigma2;
    c = c * c * c;
    c = Math.sqrt(c);
    if (w == 1) {
        // always between [0,1], so we can hard-code a tolerance here
        w = 1 - 1e-16;
    }
    c = c * M * w / ((1 - w) * N);
    Point3d xn, ym;
    double dx, dy, dz, d;
    for (int m = 0; m < M; m++) {
        P1[m] = 0;
    }
    for (int n = 0; n < N; n++) {
        Pt1[n] = 0;
        xn = X[n];
        double msum = 0;
        for (int m = 0; m < M; m++) {
            ym = TY[m];
            dx = xn.x - ym.x;
            dy = xn.y - ym.y;
            dz = xn.z - ym.z;
            if (sigma2 > 0) {
                d = Math.exp(-(dx * dx + dy * dy + dz * dz) / (2 * sigma2));
            } else if (tol2 > 0) {
                d = Math.exp(-(dx * dx + dy * dy + dz * dz) / (2 * tol2));
            } else {
                if (dx * dx + dy * dy + dz * dz == 0) {
                    d = 1;
                } else {
                    d = 0;
                }
            }
            P[m][n] = d;
            msum += d;
        }
        msum += c;
        if (msum == 0) {
            msum = 1;
        }
        for (int m = 0; m < M; m++) {
            P[m][n] = P[m][n] / msum;
            Pt1[n] += P[m][n];
            P1[m] += P[m][n];
            Np += P[m][n];
        }
    }
    return Np;
}

Example 58 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class CPD method computeVariance.

/**
 * Estimates the CPD variance
 * @param X N input points
 * @param TY M transformed output points
 * @param P probability matrix (if null, P(m,n) = 1/M)
 * @param Np Total sum of entries in probability matrix
 * @return the estimated variance
 */
public static double computeVariance(Point3d[] X, Point3d[] TY, double[][] P, double Np) {
    double var = 0;
    double M = TY.length;
    double N = X.length;
    Point3d xn, ym;
    double dx, dy, dz;
    if (P == null) {
        for (int n = 0; n < N; n++) {
            xn = X[n];
            for (int m = 0; m < M; m++) {
                ym = TY[m];
                dx = xn.x - ym.x;
                dy = xn.y - ym.y;
                dz = xn.z - ym.z;
                var += (dx * dx + dy * dy + dz * dz);
            }
        }
        var = var / (3 * N * M);
    } else {
        for (int n = 0; n < N; n++) {
            xn = X[n];
            for (int m = 0; m < M; m++) {
                ym = TY[m];
                dx = xn.x - ym.x;
                dy = xn.y - ym.y;
                dz = xn.z - ym.z;
                var += P[m][n] * (dx * dx + dy * dy + dz * dz);
            }
        }
        var = var / (3 * Np);
    }
    return var;
}

Example 59 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class CPD method affine.

/**
 * Uses the affine CPD algorithm to align two meshes
 * @param meshRef reference mesh
 * @param meshReg mesh to register
 * @param w weight, accounting to noise (w=0 --> no noise)
 * @param tol will iterative until objective function changes by less than this
 * @param maxIters maximum number of iterations
 * @return the scaled rigid transform for registration
 */
public static AffineTransform3d affine(PolygonalMesh meshRef, PolygonalMesh meshReg, double w, double tol, int maxIters) {
    int N = meshRef.numVertices();
    int M = meshReg.numVertices();
    Point3d[] x = new Point3d[N];
    Point3d[] y = new Point3d[M];
    Point3d[] match = new Point3d[M];
    for (int n = 0; n < N; n++) {
        x[n] = meshRef.getVertices().get(n).getWorldPoint();
    }
    for (int m = 0; m < M; m++) {
        y[m] = meshReg.getVertices().get(m).getWorldPoint();
        match[m] = new Point3d();
    }
    return affine(x, y, w, tol, maxIters, match, null, null);
}

Example 60 with Point3d

use of maspack.matrix.Point3d in project artisynth_core by artisynth.

the class CPD method computeQ.

/**
 * CPD Objective function
 * @param X reference points
 * @param TY transformed input points
 * @param P probability matrix
 * @param Np sum of all probabilities
 * @param sigma2 probability variance
 * @param tol2 squared point tolerance
 * @return the objective function value
 */
public static double computeQ(Point3d[] X, Point3d[] TY, double[][] P, double Np, double sigma2, double tol2) {
    double Q = 0;
    double M = TY.length;
    double N = X.length;
    double dx, dy, dz;
    Point3d xn, ym;
    for (int m = 0; m < M; m++) {
        ym = TY[m];
        for (int n = 0; n < N; n++) {
            xn = X[n];
            dx = xn.x - ym.x;
            dy = xn.y - ym.y;
            dz = xn.z - ym.z;
            Q += P[m][n] * (dx * dx + dy * dy + dz * dz);
        }
    }
    if (sigma2 > 0) {
        Q = Q / (2 * sigma2) + Np * 3 / 2 * Math.log(sigma2);
    } else if (tol2 > 0) {
        Q = Q / (2 * tol2) + Np * 3 / 2 * Math.log(tol2);
    } else {
        Q = Q / (2e-12) + Np * 3 / 2 * Math.log(1e-12);
    }
    return Q;
}