@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));
}
/**
* 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 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));
}
private static QuadraticProgram createRandomQuadraticProgram(
Random random, int objectiveSize, int solutionSize, int constraintSize) {
DenseMatrix64F a = RandomMatrices.createRandom(objectiveSize, solutionSize, random);
DenseMatrix64F b = RandomMatrices.createRandom(objectiveSize, 1, random);
DenseMatrix64F c = RandomMatrices.createRandom(constraintSize, solutionSize, random);
DenseMatrix64F d = RandomMatrices.createRandom(constraintSize, 1, random);
return new QuadraticProgram(a, b, c, d);
}
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));
}
/** 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) {
}
}
/** Depending on if setZero being true or not the size of the R matrix changes */
@Test
public void checkGetRInputSize() {
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());
// check the case where it creates the matrix first
assertTrue(alg.getR(null, true).numRows == width);
assertTrue(alg.getR(null, false).numRows == height);
// check the case where a matrix is provided
alg.getR(new DenseMatrix64F(width, width), true);
alg.getR(new DenseMatrix64F(height, width), false);
// check some negative cases
try {
alg.getR(new DenseMatrix64F(height, width), true);
fail("Should have thrown an exception");
} catch (IllegalArgumentException e) {
}
try {
alg.getR(new DenseMatrix64F(width - 1, width), false);
fail("Should have thrown an exception");
} catch (IllegalArgumentException e) {
}
}
/**
* 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);
}
@DeployableTestMethod(estimatedDuration = 0.3)
@Test(timeout = 30000)
public void test() {
Random random = new Random(176L);
int matrixSize = 5;
YoVariableRegistry parentRegistry = new YoVariableRegistry("test");
DenseMatrix64F jacobianMatrix = RandomMatrices.createRandom(matrixSize, matrixSize, random);
DenseMatrix64F taskVelocity = RandomMatrices.createRandom(matrixSize, 1, random);
DampedLeastSquaresJacobianSolver solver =
new DampedLeastSquaresJacobianSolver("testSolver", matrixSize, parentRegistry);
solver.setAlpha(1.0);
DenseMatrix64F jointVelocities = new DenseMatrix64F(matrixSize, 1);
solver.setJacobian(jacobianMatrix);
solver.solve(jointVelocities, taskVelocity);
DenseMatrix64F taskVelocityBack = new DenseMatrix64F(matrixSize, 1);
solver.inverseSolve(taskVelocityBack, jointVelocities);
double delta = 1e-9;
JUnitTools.assertMatrixEquals(taskVelocity, taskVelocityBack, delta);
}
/**
* Compare the determinant computed from LU to the value computed from the minor matrix method.
*/
@Test
public void testDeterminant() {
Random rand = new Random(0xfff);
int width = 10;
DenseMatrix64F A = RandomMatrices.createRandom(width, width, rand);
DeterminantFromMinor minor = new DeterminantFromMinor(width);
double minorVal = minor.compute(A);
LUDecompositionAlt alg = new LUDecompositionAlt();
alg.decompose(A);
double luVal = alg.computeDeterminant();
assertEquals(minorVal, luVal, 1e-6);
}
@Test
public void _solveVectorInternal() {
int width = 10;
DenseMatrix64F LU = RandomMatrices.createRandom(width, width, rand);
double a[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
double b[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
for (int i = 0; i < width; i++) LU.set(i, i, 1);
TriangularSolver.solveL(LU.data, a, width);
TriangularSolver.solveU(LU.data, a, width);
DebugDecompose alg = new DebugDecompose(width);
for (int i = 0; i < width; i++) alg.getIndx()[i] = i;
alg.setLU(LU);
alg._solveVectorInternal(b);
for (int i = 0; i < width; i++) {
assertEquals(a[i], b[i], 1e-6);
}
}
