Esempio n. 1
0
  /**
   * 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));
  }
Esempio n. 2
0
  @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));
  }
Esempio n. 4
0
  /** 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) {
    }
  }
Esempio n. 5
0
  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));
  }
Esempio n. 6
0
  /** 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));
      }
    }
  }
Esempio n. 7
0
  @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));
  }
Esempio n. 13
0
  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);
  }
Esempio n. 14
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));
  }
Esempio n. 17
0
  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);
  }
Esempio n. 18
0
 /**
  * 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);
 }
Esempio n. 19
0
 /**
  * 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);
 }