/** 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 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));
}
@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));
}
/** 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));
}
@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);
}
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));
}
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);
}
