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

Example 61 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 P Sequence of n-dimensional points.
 * @param Q Sequence of n-dimensional points (reference).
 * @return The total error for the estimated fit.
 */
private double getEuclideanError(Pnt2d[] P, Pnt2d[] Q) {
    int m = Math.min(P.length, Q.length);
    RealMatrix sR = R.scalarMultiply(s);
    double errSum = 0;
    for (int i = 0; i < m; i++) {
        RealVector p = new ArrayRealVector(P[i].toDoubleArray());
        RealVector q = new ArrayRealVector(Q[i].toDoubleArray());
        RealVector pp = sR.operate(p).add(t);
        // System.out.format("p=%s, q=%s, pp=%s\n", p.toString(), q.toString(), pp.toString());
        double e = pp.subtract(q).getNorm();
        errSum = errSum + e * e;
    }
    // correct!
    return Math.sqrt(errSum);
}

Example 62 with RealVector

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

the class FitPoints method solveSystem.

/**
 * Solve the polynomial expression Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz +
 * 2Gx + 2Hy + 2Iz from the provided points.
 *
 * @param points the points that will be fit to the polynomial expression.
 * @return the solution vector to the polynomial expression.
 */
private RealVector solveSystem(ArrayList<ThreeSpacePoint> points) {
    // determine the number of points
    int numPoints = points.size();
    // the design matrix
    // size: numPoints x 9
    RealMatrix d = new Array2DRowRealMatrix(numPoints, 9);
    // Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz
    for (int i = 0; i < d.getRowDimension(); i++) {
        double xx = Math.pow(points.get(i).x, 2);
        double yy = Math.pow(points.get(i).y, 2);
        double zz = Math.pow(points.get(i).z, 2);
        double xy = 2 * (points.get(i).x * points.get(i).y);
        double xz = 2 * (points.get(i).x * points.get(i).z);
        double yz = 2 * (points.get(i).y * points.get(i).z);
        double x = 2 * points.get(i).x;
        double y = 2 * points.get(i).y;
        double z = 2 * points.get(i).z;
        d.setEntry(i, 0, xx);
        d.setEntry(i, 1, yy);
        d.setEntry(i, 2, zz);
        d.setEntry(i, 3, xy);
        d.setEntry(i, 4, xz);
        d.setEntry(i, 5, yz);
        d.setEntry(i, 6, x);
        d.setEntry(i, 7, y);
        d.setEntry(i, 8, z);
    }
    // solve the normal system of equations
    // v = (( d' * d )^-1) * ( d' * ones.mapAddToSelf(1));
    // Multiply: d' * d
    RealMatrix dtd = d.transpose().multiply(d);
    // Create a vector of ones.
    RealVector ones = new ArrayRealVector(numPoints);
    ones.mapAddToSelf(1);
    // Multiply: d' * ones.mapAddToSelf(1)
    RealVector dtOnes = d.transpose().operate(ones);
    // Find ( d' * d )^-1
    DecompositionSolver solver = new SingularValueDecomposition(dtd).getSolver();
    RealMatrix dtdi = solver.getInverse();
    // v = (( d' * d )^-1) * ( d' * ones.mapAddToSelf(1));
    return dtdi.operate(dtOnes);
}

Example 63 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);
}