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

Example 1 with ScaledRigidTransform3d

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

the class CPDTest method main.

public static void main(String[] args) {
    AffineTransform3d trans = new AffineTransform3d();
    RotationMatrix3d R = new RotationMatrix3d(0.7605, -0.6307, 0.1541, 0.6485, 0.7263, -0.2279, 0.0318, 0.2733, 0.9614);
    double s = 2.7;
    trans.setA(R, new Vector3d(s, s, s), new Vector3d(0, 0, 0));
    trans.setTranslation(new Vector3d(1, 2, 3));
    Point3d[] X = get3DFish();
    int N = X.length;
    int M = N - 20;
    Point3d[] Y = new Point3d[M];
    Point3d[] out = new Point3d[M];
    for (int i = 0; i < N; i++) {
        if (i < M) {
            Y[i] = new Point3d(X[i]);
            out[i] = new Point3d();
        }
        X[i].transform(trans);
    }
    double w = 0.01;
    double lambda = 0.1;
    double beta2 = 3.5;
    CPD.verbose = true;
    ScaledRigidTransform3d rigidT = CPD.rigid(X, Y, w, 1e-10, 100, true, out);
    AffineTransform3d affT = CPD.affine(X, Y, w, 1e-10, 100, out);
    CPD.coherent(X, Y, lambda, beta2, w, 1e-10, 100, out);
    System.out.println("Original: \n" + trans.toString());
    System.out.println("Rigid Computed: \n" + rigidT.toString());
    System.out.println("Affine Computed: \n" + affT.toString());
    for (int i = 0; i < M; i++) {
        System.out.println(X[i].toString() + "\n" + out[i].toString() + "\n");
    }
}

Example 2 with ScaledRigidTransform3d

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

the class CPD method rigid.

/**
 * Uses the rigid CPD algorithm to align a set of points
 * @param X reference input points
 * @param Y points 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
 * @param allowScaling whether or not to allow scaling
 * @param TY transformed points
 * @param trans initial guess of scaled rigid transform
 * @param sigma2Holder initial guess of variance
 * @return the scaled rigid transform for registration
 */
public static ScaledRigidTransform3d rigid(Point3d[] X, Point3d[] Y, double w, double tol, int maxIters, boolean allowScaling, Point3d[] TY, ScaledRigidTransform3d trans, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    if (trans == null) {
        trans = new ScaledRigidTransform3d();
        transformPoints(Y, TY);
    } else {
        transformPoints(Y, trans, TY);
    }
    double sigma2;
    if (sigma2Holder == null || sigma2Holder[0] < 0) {
        sigma2 = computeVariance(X, TY, null, 1.0 / M);
    } else {
        sigma2 = sigma2Holder[0];
    }
    SVDecomposition3d svd = new SVDecomposition3d();
    Matrix3d R = new Matrix3d(trans.R);
    Vector3d t = new Vector3d(trans.p);
    double s = trans.s;
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    double[] tr = new double[2];
    Matrix3d A = new Matrix3d();
    Matrix3d UVt = new Matrix3d();
    Matrix3d C = new Matrix3d();
    C.set(0, 0, 1);
    C.set(1, 1, 1);
    Point3d meanx = new Point3d();
    Point3d meany = new Point3d();
    double err = Double.MAX_VALUE;
    int iters = 0;
    double q, qprev;
    q = Double.MAX_VALUE;
    // iterative part of algorithm
    while ((iters < maxIters) && (err > tol)) {
        // E-step
        Np = computeP(X, TY, sigma2, w, P, P1, Pt1);
        // M-step
        // mean
        computeMean(X, Pt1, Np, meanx);
        computeMean(Y, P1, Np, meany);
        // A = (X-mean(X))'*P'*(Y-mean(Y))
        // d = trace( trace(Y'*diag(P1)*Y) );
        computeAD(X, meanx, P, P1, Pt1, Y, meany, A, tr);
        // R = U*C*V', C= diag([1 1 det(U*V')])
        svd.factor(A);
        UVt.set(svd.getU());
        UVt.mulTranspose(svd.getV());
        C.set(2, 2, UVt.determinant());
        R.set(svd.getU());
        R.mul(C);
        R.mulTranspose(svd.getV());
        // s = trace(A'*R)/trace(Y'*diag(P1)*Y)
        A.mulTransposeLeft(A, R);
        double trAtR = A.trace();
        if (allowScaling) {
            s = trAtR / tr[1];
        }
        // t = mean(X)-s*R*mean(Y)
        t.mul(R, meany);
        t.scale(-s);
        t.add(meanx);
        // Adjust output
        transformPoints(Y, s, R, t, TY);
        // New objective function
        qprev = q;
        // q = computeQ(X, TY, P, Np, sigma2);
        q = (tr[0] - 2 * s * trAtR + s * s * tr[1]) / (2 * sigma2) + 1.5 * Np * Math.log(sigma2);
        // new variance estimate
        // sigma2 = computeVariance(X, TY, P, Np);
        sigma2 = (tr[0] - s * trAtR) / (3 * Np);
        if (sigma2 <= 0) {
            sigma2 = tol;
        }
        err = Math.abs(q - qprev);
        iters++;
    }
    if (verbose) {
        System.out.println("Registration complete in " + iters + " iterations");
    }
    // copy info over
    trans.R.set(R);
    trans.p.set(t);
    // triggers update of internal matrix
    trans.setScale(s);
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return trans;
}

Example 3 with ScaledRigidTransform3d

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

the class CPDRigidAligner method main.

public static void main(String[] args) {
    String file1 = args[0];
    String file2 = args[1];
    PolygonalMesh mesh1, mesh2;
    try {
        mesh1 = new PolygonalMesh(new File(file1));
        mesh2 = new PolygonalMesh(new File(file2));
    } catch (IOException e) {
        e.printStackTrace();
        return;
    }
    ScaledRigidTransform3d trans = CPD.rigid(mesh1, mesh2, 0, 0, 100, false);
    System.out.println(trans.toString());
}