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

Example 6 with SVDecomposition3d

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

the class GLViewer method computeInverseTranspose.

protected Matrix3d computeInverseTranspose(Matrix3dBase M) {
    Matrix3d out = new Matrix3d(M);
    if (!(M instanceof RotationMatrix3d)) {
        boolean success = out.invert();
        if (!success) {
            SVDecomposition3d svd3 = new SVDecomposition3d(M);
            svd3.pseudoInverse(out);
        }
        out.transpose();
    }
    return out;
}

Example 7 with SVDecomposition3d

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

the class StlRenderer method computeInverseTranspose.

protected Matrix3d computeInverseTranspose(Matrix3dBase M) {
    Matrix3d out = new Matrix3d(M);
    if (!(M instanceof RotationMatrix3d)) {
        boolean success = out.invert();
        if (!success) {
            SVDecomposition3d svd3 = new SVDecomposition3d(M);
            svd3.pseudoInverse(out);
        }
        out.transpose();
    }
    return out;
}

Example 8 with SVDecomposition3d

use of maspack.matrix.SVDecomposition3d 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 9 with SVDecomposition3d

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

the class CPD method affine.

/**
 * Uses the affine 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 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 AffineTransform3d affine(Point3d[] X, Point3d[] Y, double w, double tol, int maxIters, Point3d[] TY, AffineTransform3d trans, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    SVDecomposition3d svd = new SVDecomposition3d();
    if (trans == null) {
        trans = new AffineTransform3d();
        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];
    }
    Matrix3d B = new Matrix3d(trans.A);
    Vector3d t = new Vector3d(trans.p);
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    Matrix3d A = new Matrix3d();
    Matrix3d D = new Matrix3d();
    Matrix3d YPY = new Matrix3d();
    double[] tr = new double[2];
    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 = (Y-mean(Y))'*diag(P1)*(Y-mean(Y))
        computeAD(X, meanx, P, P1, Pt1, Y, meany, A, YPY, tr);
        // B = A*inverse(D)
        svd.factor(YPY);
        // pseudo-invert
        svd.pseudoInverse(D);
        B.mul(A, D);
        // t = mean(X)-A*mean(Y)
        t.mul(B, meany);
        t.sub(meanx, t);
        // Adjust output
        transformPoints(Y, B, t, TY);
        // speedy compute Q and sigma2
        A.mulTranspose(B);
        double trABt = A.trace();
        YPY.mulTranspose(B);
        YPY.mul(B);
        double trBYPYB = YPY.trace();
        // compute new objective (speedy)
        qprev = q;
        // q = computeQ(X, TY, P, Np, sigma2);
        q = (tr[0] - 2 * trABt + trBYPYB) / (2 * sigma2) + 1.5 * Np * Math.log(sigma2);
        // new variance estimate (speedy)
        // sigma2 = computeVariance(X, TY, P, Np);
        sigma2 = (tr[0] - trABt) / (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.A.set(B);
    trans.p.set(t);
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return trans;
}