Examples with RealVector org.apache.commons.math3.linear.RealVector used on opensource projects

Example 46 with RealVector

use of org.apache.commons.math3.linear.RealVector in project imagingbook-common by imagingbook.

the class ProcrustesFit method fit.

@Override
public void fit(List<double[]> X, List<double[]> Y) {
    if (X.size() != Y.size())
        throw new IllegalArgumentException("point sequences X, Y must have same length");
    this.m = X.size();
    this.n = X.get(0).length;
    double[] meanX = null;
    double[] meanY = null;
    if (this.allowTranslation) {
        meanX = getMeanVec(X);
        meanY = getMeanVec(Y);
    }
    RealMatrix P = makeDataMatrix(X, meanX);
    RealMatrix Q = makeDataMatrix(Y, meanY);
    // P, Q of same dimensions?
    MatrixUtils.checkAdditionCompatible(P, Q);
    RealMatrix QPt = Q.multiply(P.transpose());
    SingularValueDecomposition svd = new SingularValueDecomposition(QPt);
    RealMatrix U = svd.getU();
    RealMatrix S = svd.getS();
    RealMatrix V = svd.getV();
    double d = (svd.getRank() >= n) ? det(QPt) : det(U) * det(V);
    RealMatrix D = MatrixUtils.createRealIdentityMatrix(n);
    if (d < 0 && forceRotation)
        D.setEntry(n - 1, n - 1, -1);
    R = U.multiply(D).multiply(V.transpose());
    double normP = P.getFrobeniusNorm();
    double normQ = Q.getFrobeniusNorm();
    c = (this.allowScaling) ? S.multiply(D).getTrace() / sqr(normP) : 1.0;
    if (allowTranslation) {
        RealVector ma = MatrixUtils.createRealVector(meanX);
        RealVector mb = MatrixUtils.createRealVector(meanY);
        t = mb.subtract(R.scalarMultiply(c).operate(ma));
    } else {
        // zero vector
        t = new ArrayRealVector(n);
    }
    err = sqr(normQ) - sqr(S.multiply(D).getTrace() / normP);
}

Example 47 with RealVector

use of org.apache.commons.math3.linear.RealVector in project imagingbook-common by imagingbook.

the class ProcrustesFit method getEuclideanError.

/**
 * Calculates the total error for the estimated fit as
 * the sum of the squared Euclidean distances between the
 * transformed point set X and the reference set Y.
 * This method is provided for testing as an alternative to
 * the quicker {@link getError} method.
 * @param X Sequence of n-dimensional points.
 * @param Y Sequence of n-dimensional points (reference).
 * @return The total error for the estimated fit.
 */
public double getEuclideanError(List<double[]> X, List<double[]> Y) {
    RealMatrix cR = R.scalarMultiply(c);
    double ee = 0;
    for (int i = 0; i < X.size(); i++) {
        RealVector ai = new ArrayRealVector(X.get(i));
        RealVector bi = new ArrayRealVector(Y.get(i));
        RealVector aiT = cR.operate(ai).add(t);
        double ei = aiT.subtract(bi).getNorm();
        ee = ee + sqr(ei);
    }
    return ee;
}

Example 48 with RealVector

use of org.apache.commons.math3.linear.RealVector in project imagingbook-common by imagingbook.

the class Matrix method solve.

// use general method, i.e. double[][] inverse(double[][] A)
// @Deprecated
// public static double[][] inverse3x3(final double[][] A) {
// double[][] B = duplicate(A);
// final double det = determinant3x3(B);
// if (Math.abs(det) < Arithmetic.EPSILON_DOUBLE)
// return null;
// else {
// final double a00 = B[0][0];
// final double a01 = B[0][1];
// final double a02 = B[0][2];
// final double a10 = B[1][0];
// final double a11 = B[1][1];
// final double a12 = B[1][2];
// final double a20 = B[2][0];
// final double a21 = B[2][1];
// final double a22 = B[2][2];
// B[0][0] = (a11 * a22 - a12 * a21) / det;
// B[0][1] = (a02 * a21 - a01 * a22) / det;
// B[0][2] = (a01 * a12 - a02 * a11) / det;
// 
// B[1][0] = (a12 * a20 - a10 * a22) / det;
// B[1][1] = (a00 * a22 - a02 * a20) / det;
// B[1][2] = (a02 * a10 - a00 * a12) / det;
// 
// B[2][0] = (a10 * a21 - a11 * a20) / det;
// B[2][1] = (a01 * a20 - a00 * a21) / det;
// B[2][2] = (a00 * a11 - a01 * a10) / det;
// return B;
// }
// }
// numerically stable? should be replaced by standard inversion
// @Deprecated
// public static float[][] inverse3x3(final float[][] A) {
// final float[][] B = duplicate(A);
// final double det = determinant3x3(B);
// // IJ.log("   determinant = " + det);
// if (Math.abs(det) < Arithmetic.EPSILON_DOUBLE)
// return null;
// else {
// final double a00 = B[0][0];
// final double a01 = B[0][1];
// final double a02 = B[0][2];
// final double a10 = B[1][0];
// final double a11 = B[1][1];
// final double a12 = B[1][2];
// final double a20 = B[2][0];
// final double a21 = B[2][1];
// final double a22 = B[2][2];
// B[0][0] = (float) ((a11 * a22 - a12 * a21) / det);
// B[0][1] = (float) ((a02 * a21 - a01 * a22) / det);
// B[0][2] = (float) ((a01 * a12 - a02 * a11) / det);
// 
// B[1][0] = (float) ((a12 * a20 - a10 * a22) / det);
// B[1][1] = (float) ((a00 * a22 - a02 * a20) / det);
// B[1][2] = (float) ((a02 * a10 - a00 * a12) / det);
// 
// B[2][0] = (float) ((a10 * a21 - a11 * a20) / det);
// B[2][1] = (float) ((a01 * a20 - a00 * a21) / det);
// B[2][2] = (float) ((a00 * a11 - a01 * a10) / det);
// return B;
// }
// }
// ------------------------------------------------------------------------
// Finds the EXACT solution x for A.x = b
public static double[] solve(final double[][] A, double[] b) {
    RealMatrix AA = MatrixUtils.createRealMatrix(A);
    RealVector bb = MatrixUtils.createRealVector(b);
    DecompositionSolver solver = new LUDecomposition(AA).getSolver();
    double[] x = null;
    try {
        x = solver.solve(bb).toArray();
    } catch (SingularMatrixException e) {
    }
    return x;
}

Example 49 with RealVector

use of org.apache.commons.math3.linear.RealVector in project FSensor by KalebKE.

the class RotationKalmanFilter method correct.

/**
 * Correct the current state estimate with an actual measurement.
 *
 * @param z
 *            the measurement vector
 * @throws NullArgumentException
 *             if the measurement vector is {@code null}
 * @throws DimensionMismatchException
 *             if the dimension of the measurement vector does not fit
 * @throws SingularMatrixException
 *             if the covariance matrix could not be inverted
 */
public void correct(final RealVector z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
    // sanity checks
    MathUtils.checkNotNull(z);
    if (z.getDimension() != measurementMatrix.getRowDimension()) {
        throw new DimensionMismatchException(z.getDimension(), measurementMatrix.getRowDimension());
    }
    // S = H * P(k) * H' + R
    RealMatrix s = measurementMatrix.multiply(errorCovariance).multiply(measurementMatrixT).add(measurementModel.getMeasurementNoise());
    // Inn = z(k) - H * xHat(k)-
    RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));
    // calculate gain matrix
    // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
    // K(k) = P(k)- * H' * S^-1
    // instead of calculating the inverse of S we can rearrange the formula,
    // and then solve the linear equation A x X = B with A = S', X = K' and
    // B = (H * P)'
    // K(k) * S = P(k)- * H'
    // S' * K(k)' = H * P(k)-'
    RealMatrix kalmanGain = new CholeskyDecomposition(s).getSolver().solve(measurementMatrix.multiply(errorCovariance.transpose())).transpose();
    // update estimate with measurement z(k)
    // xHat(k) = xHat(k)- + K * Inn
    stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));
    // update covariance of prediction error
    // P(k) = (I - K * H) * P(k)-
    RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
    errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
}

Example 50 with RealVector

use of org.apache.commons.math3.linear.RealVector in project narchy by automenta.

the class HordeAgent method getAtp1.

public A getAtp1(RealVector o_tp1, A a, double r_tp1) {
    /*if (step.isEpisodeStarting())
            x_t = null;*/
    RealVector x_tp1 = projector.project(o_tp1);
    A a_tp1 = control.step(x_t, a, x_tp1, r_tp1);
    horde.update(o_tp1, x_t, a, x_tp1);
    x_t = x_tp1;
    return a_tp1;
}