/**
* See if passing in a matrix or not providing one to getQ and getR functions has the same result
*/
@Test
public void checkGetNullVersusNot() {
int width = 5;
int height = 10;
QRDecomposition<DenseMatrix64F> alg = createQRDecomposition();
SimpleMatrix A = new SimpleMatrix(height, width);
RandomMatrices.setRandom(A.getMatrix(), rand);
alg.decompose(A.getMatrix());
// get the results from a provided matrix
DenseMatrix64F Q_provided = RandomMatrices.createRandom(height, height, rand);
DenseMatrix64F R_provided = RandomMatrices.createRandom(height, width, rand);
assertTrue(R_provided == alg.getR(R_provided, false));
assertTrue(Q_provided == alg.getQ(Q_provided, false));
// get the results when no matrix is provided
DenseMatrix64F Q_null = alg.getQ(null, false);
DenseMatrix64F R_null = alg.getR(null, false);
// see if they are the same
assertTrue(MatrixFeatures.isEquals(Q_provided, Q_null));
assertTrue(MatrixFeatures.isEquals(R_provided, R_null));
}
@Test
public void removeRowFromA() {
int remove = 2;
int m = 5;
int n = 3;
DenseMatrix64F A = RandomMatrices.createRandom(m, n, rand);
// create the modified A
DenseMatrix64F A_e = RandomMatrices.createRandom(m - 1, n, rand);
SubmatrixOps.setSubMatrix(A, A_e, 0, 0, 0, 0, remove, n);
SubmatrixOps.setSubMatrix(A, A_e, remove + 1, 0, remove, 0, m - remove - 1, n);
// Compute the solution to the modified system
DenseMatrix64F X = RandomMatrices.createRandom(n, 2, rand);
DenseMatrix64F Y = new DenseMatrix64F(A_e.numRows, X.numCols);
CommonOps.mult(A_e, X, Y);
// create the solver from the original system then modify it
AdjustableLinearSolver adjSolver = new AdjLinearSolverQr();
adjSolver.setA(A);
adjSolver.removeRowFromA(remove);
// see if it produces the epected results
// solve the system and see if it gets the expected solution
DenseMatrix64F X_found = RandomMatrices.createRandom(X.numRows, X.numCols, rand);
adjSolver.solve(Y, X_found);
// see if they produce the same results
assertTrue(MatrixFeatures.isIdentical(X_found, X, 1e-8));
}
@DeployableTestMethod(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testConstrainedSimple() {
int objectiveSize = 3;
int solutionSize = 3;
int constraintSize = 1;
DenseMatrix64F a = new DenseMatrix64F(objectiveSize, solutionSize);
CommonOps.setIdentity(a);
DenseMatrix64F b = new DenseMatrix64F(objectiveSize, 1);
b.zero();
DenseMatrix64F c = new DenseMatrix64F(constraintSize, solutionSize);
CommonOps.setIdentity(c);
DenseMatrix64F d = new DenseMatrix64F(constraintSize, 1);
d.set(0, 0, -1.0);
QuadraticProgram quadraticProgram = new QuadraticProgram(a, b, c, d);
DenseMatrix64F initialGuess = new DenseMatrix64F(solutionSize, 1);
initialGuess.set(0, 0, -10.0);
initialGuess.set(1, 0, -10.0);
initialGuess.set(2, 0, -10.0);
ActiveSearchSolutionInfo solutionInfo = solve(quadraticProgram, initialGuess);
assertTrue(solutionInfo.isConverged());
DenseMatrix64F expectedResult = new DenseMatrix64F(solutionSize, 1);
expectedResult.set(0, 0, d.get(0, 0));
expectedResult.set(1, 0, 0.0);
expectedResult.set(2, 0, 0.0);
assertTrue(MatrixFeatures.isEquals(expectedResult, solutionInfo.getSolution(), 1e-12));
}
/** See if the compact format for Q works */
@Test
public void checkCompactFormat() {
int height = 10;
int width = 5;
QRDecomposition<DenseMatrix64F> alg = createQRDecomposition();
SimpleMatrix A = new SimpleMatrix(height, width);
RandomMatrices.setRandom(A.getMatrix(), rand);
alg.decompose(A.getMatrix());
SimpleMatrix Q = new SimpleMatrix(height, width);
alg.getQ(Q.getMatrix(), true);
// see if Q has the expected properties
assertTrue(MatrixFeatures.isOrthogonal(Q.getMatrix(), 1e-6));
// try to extract it with the wrong dimensions
Q = new SimpleMatrix(height, height);
try {
alg.getQ(Q.getMatrix(), true);
fail("Didn't fail");
} catch (RuntimeException e) {
}
}
private void checkDecomposition(int height, int width, boolean compact) {
QRDecomposition<DenseMatrix64F> alg = createQRDecomposition();
SimpleMatrix A = new SimpleMatrix(height, width);
RandomMatrices.setRandom(A.getMatrix(), rand);
assertTrue(alg.decompose(A.copy().getMatrix()));
int minStride = Math.min(height, width);
SimpleMatrix Q = new SimpleMatrix(height, compact ? minStride : height);
alg.getQ(Q.getMatrix(), compact);
SimpleMatrix R = new SimpleMatrix(compact ? minStride : height, width);
alg.getR(R.getMatrix(), compact);
// see if Q has the expected properties
assertTrue(MatrixFeatures.isOrthogonal(Q.getMatrix(), 1e-6));
// UtilEjml.print(alg.getQR());
// Q.print();
// R.print();
// see if it has the expected properties
DenseMatrix64F A_found = Q.mult(R).getMatrix();
EjmlUnitTests.assertEquals(A.getMatrix(), A_found, 1e-6);
assertTrue(Q.transpose().mult(A).isIdentical(R, 1e-6));
}
/** Call it multiple times and make sure the same solutions are returned. */
@Test
public void checkMultipleCalls() {
List<Point2D3D> inputs = createObservations(worldToCamera0, alg.getMinimumPoints());
assertTrue(alg.process(inputs, solutions));
assertTrue(solutions.size() > 0);
List<Se3_F64> orig = new ArrayList<Se3_F64>();
for (Se3_F64 m : solutions.toList()) {
orig.add(m.copy());
}
// run it a few times and see if the output is identical
for (int i = 0; i < 2; i++) {
assertTrue(alg.process(inputs, solutions));
assertEquals(orig.size(), solutions.size());
for (int j = 0; j < orig.size(); j++) {
Se3_F64 o = orig.get(j);
Se3_F64 f = solutions.get(j);
assertTrue(MatrixFeatures.isIdentical(o.getR(), f.getR(), 1e-8));
assertTrue(f.getT().isIdentical(o.getT(), 1e-8));
}
}
}
@Test
public void addRowToA() {
int insert = 2;
int m = 5;
int n = 3;
DenseMatrix64F A = RandomMatrices.createRandom(m, n, rand);
double row[] = new double[] {1, 2, 3};
// create the modified A
DenseMatrix64F A_e = RandomMatrices.createRandom(m + 1, n, rand);
SubmatrixOps.setSubMatrix(A, A_e, 0, 0, 0, 0, insert, n);
System.arraycopy(row, 0, A_e.data, insert * n, n);
SubmatrixOps.setSubMatrix(A, A_e, insert, 0, insert + 1, 0, m - insert, n);
// Compute the solution to the modified system
DenseMatrix64F X = RandomMatrices.createRandom(n, 2, rand);
DenseMatrix64F Y = new DenseMatrix64F(A_e.numRows, X.numCols);
CommonOps.mult(A_e, X, Y);
// create the solver from A then add a A. The solver
// should be equivalent to one created from A_e
AdjustableLinearSolver adjSolver = new AdjLinearSolverQr();
assertTrue(adjSolver.setA(A));
adjSolver.addRowToA(row, insert);
// solve the system and see if it gets the expected solution
DenseMatrix64F X_found = RandomMatrices.createRandom(X.numRows, X.numCols, rand);
adjSolver.solve(Y, X_found);
// see if they produce the same results
assertTrue(MatrixFeatures.isIdentical(X_found, X, 1e-8));
}
/**
* Checks to see if the matrix is or is not modified as according to the modified flag.
*
* @param decomp
*/
public static void checkModifiedInput(DecompositionInterface decomp) {
DenseMatrix64F A = RandomMatrices.createSymmPosDef(4, new Random(0x434));
DenseMatrix64F A_orig = A.copy();
assertTrue(decomp.decompose(A));
boolean modified = !MatrixFeatures.isEquals(A, A_orig);
assertTrue(modified + " " + decomp.inputModified(), decomp.inputModified() == modified);
}
/** Sees if both crossMatrix functions produce the same output */
@Test
public void crossMatrix_sameOut() {
double a = 1.1, b = -0.5, c = 2.2;
Vector3D_F64 v = new Vector3D_F64(a, b, c);
DenseMatrix64F V1 = GeometryMath_F64.crossMatrix(v, null);
DenseMatrix64F V2 = GeometryMath_F64.crossMatrix(a, b, c, null);
assertTrue(MatrixFeatures.isIdentical(V1, V2, 1e-8));
}
@Override
public OutputError checkResults(BenchmarkMatrix[] output, double tol) {
if (output[0] == null) {
return OutputError.MISC;
}
DenseMatrix64F o = RandomizeMatrices.convertToEjml(output[0]);
if (!MatrixFeatures.isInverse(o, A, 1e-8)) return OutputError.LARGE_ERROR;
return OutputError.NO_ERROR;
}
@Test
public void outerProd_3D() {
Vector3D_F64 a = new Vector3D_F64(2, -2, 5);
Vector3D_F64 b = new Vector3D_F64(4, 3, 9);
DenseMatrix64F M = new DenseMatrix64F(3, 3, true, 1, 2, 3, 4, 5, 6, 7, 8, 9);
DenseMatrix64F expected = new DenseMatrix64F(3, 3, true, 8, 6, 18, -8, -6, -18, 20, 15, 45);
GeometryMath_F64.outerProd(a, b, M);
assertTrue(MatrixFeatures.isIdentical(expected, M, GrlConstants.DOUBLE_TEST_TOL));
}
@Test
public void multCrossA_2d() {
Vector2D_F64 a = new Vector2D_F64(-1, 2);
DenseMatrix64F M = new DenseMatrix64F(3, 3, true, 1, 2, 3, 4, 5, 6, 7, 8, 9);
DenseMatrix64F a_hat = GeometryMath_F64.crossMatrix(a.x, a.y, 1, null);
DenseMatrix64F expected = new DenseMatrix64F(3, 3);
CommonOps.mult(a_hat, M, expected);
DenseMatrix64F found = GeometryMath_F64.multCrossA(a, M, null);
assertTrue(MatrixFeatures.isIdentical(expected, found, GrlConstants.DOUBLE_TEST_TOL));
}
private void perfectObservations(Se3_F64 worldToCamera, int numSample) {
List<Point2D3D> inputs = createObservations(worldToCamera, numSample);
assertTrue(alg.process(inputs, solutions));
int numMatched = 0;
for (Se3_F64 found : solutions.toList()) {
if (!MatrixFeatures.isIdentical(worldToCamera.getR(), found.getR(), 1e-8)) continue;
if (!found.getT().isIdentical(worldToCamera.getT(), 1e-8)) continue;
numMatched++;
}
assertTrue(numMatched > 0);
}
@Test
public void perfectInput() {
init(30, false);
// compute true essential matrix
DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());
ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();
// give it the perfect matrix and see if it screwed it up
assertTrue(alg.fitModel(pairs, E, found));
// normalize so that they are the same
CommonOps.divide(E.get(2, 2), E);
CommonOps.divide(found.get(2, 2), found);
assertTrue(MatrixFeatures.isEquals(E, found, 1e-8));
}
private void perfectObservations(int numSample) {
init(numSample, false);
List<PointPosePair> inputs = new ArrayList<PointPosePair>();
for (int i = 0; i < currentObs.size(); i++) {
Point2D_F64 o = currentObs.get(i);
Point3D_F64 X = worldPts.get(i);
inputs.add(new PointPosePair(o, X));
}
Se3_F64 found = new Se3_F64();
assertTrue(alg.process(inputs, found));
assertTrue(MatrixFeatures.isIdentical(worldToCamera.getR(), found.getR(), 1e-8));
assertTrue(found.getT().isIdentical(worldToCamera.getT(), 1e-8));
}
@DeployableTestMethod(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testUnconstrained() {
Random random = new Random(12355L);
int objectiveSize = 5;
int solutionSize = 5;
int constraintSize = 0;
QuadraticProgram quadraticProgram =
createRandomQuadraticProgram(random, objectiveSize, solutionSize, constraintSize);
DenseMatrix64F initialGuess = new DenseMatrix64F(solutionSize, 1);
ActiveSearchSolutionInfo solutionInfo = solve(quadraticProgram, initialGuess);
assertTrue(solutionInfo.isConverged());
DenseMatrix64F axMinusB = new DenseMatrix64F(solutionSize, 1);
CommonOps.mult(quadraticProgram.getA(), solutionInfo.getSolution(), axMinusB);
CommonOps.subtractEquals(axMinusB, quadraticProgram.getB());
assertTrue(MatrixFeatures.isConstantVal(axMinusB, 0.0, 1e-12));
}
public static boolean jacobian(
FunctionNtoM func,
FunctionNtoMxN jacobian,
double param[],
double tol,
double differenceScale) {
NumericalJacobianForward numerical = new NumericalJacobianForward(func, differenceScale);
if (numerical.getNumOfOutputsM() != jacobian.getNumOfOutputsM())
throw new RuntimeException(
"M is not equal " + numerical.getNumOfOutputsM() + " " + jacobian.getNumOfOutputsM());
if (numerical.getNumOfInputsN() != jacobian.getNumOfInputsN())
throw new RuntimeException(
"N is not equal: " + numerical.getNumOfInputsN() + " " + jacobian.getNumOfInputsN());
DenseMatrix64F found = new DenseMatrix64F(func.getNumOfOutputsM(), func.getNumOfInputsN());
DenseMatrix64F expected = new DenseMatrix64F(func.getNumOfOutputsM(), func.getNumOfInputsN());
jacobian.process(param, found.data);
numerical.process(param, expected.data);
return MatrixFeatures.isIdentical(expected, found, tol);
}
/**
* Checks to see if matrix 'a' is the same as this matrix within the specified tolerance.
*
* @param a The matrix it is being compared against.
* @param tol How similar they must be to be equals.
* @return If they are equal within tolerance of each other.
*/
public boolean isIdentical(T a, double tol) {
return MatrixFeatures.isIdentical(mat, a.getMatrix(), tol);
}
/**
* Checks to see if any of the elements in this matrix are either NaN or infinite.
*
* @return True of an element is NaN or infinite. False otherwise.
*/
public boolean hasUncountable() {
return MatrixFeatures.hasUncountable(mat);
}
