public void evaluateObservationNoise(
      double minPixelError, double maxPixelError, int N, boolean isPlanar) {
    System.out.println("------------------------");

    rand = new Random(234);

    for (int i = 0; i <= N; i++) {
      double mag = (maxPixelError - minPixelError) * i / N + minPixelError;

      init(NUM_POINTS, false, isPlanar);
      addPixelNoise(mag);

      if (!target.process(observationPose, found))
        throw new RuntimeException("Not expected to fail");

      double expectedEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(motion.getR());
      double foundEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(found.getR());

      Vector3D_F64 expectedTran = motion.getT();
      Vector3D_F64 foundTran = found.getT();

      double errorTran = expectedTran.distance(foundTran);
      double errorEuler = 0;
      double sum = 0;
      for (int j = 0; j < 3; j++) {
        double e = expectedEuler[j] - foundEuler[j];
        errorEuler += e * e;
        sum += expectedEuler[j];
      }
      errorEuler = 100 * Math.sqrt(errorEuler) / Math.sqrt(sum);

      System.out.printf("%3d angle %6.2f%% translation %6.2e\n", i, errorEuler, errorTran);
    }
  }
  public void evaluateMinimal(double pixelSigma, boolean isPlanar, int numTrials) {

    // make sure each test case has the same random seed
    rand = new Random(234);

    double totalEuler = 0;
    double totalTran = 0;
    int totalFail = 0;

    for (int i = 0; i < numTrials; i++) {
      init(target.getMinimumPoints(), false, isPlanar);
      addPixelNoise(pixelSigma);

      if (!target.process(observationPose, found)) {
        totalFail++;
        continue;
      }

      double expectedEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(motion.getR());
      double foundEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(found.getR());

      Vector3D_F64 expectedTran = motion.getT();
      Vector3D_F64 foundTran = found.getT();

      double errorTran = expectedTran.distance(foundTran);
      double errorEuler = 0;
      double sum = 0;
      for (int j = 0; j < 3; j++) {
        double e = expectedEuler[j] - foundEuler[j];
        errorEuler += e * e;
        sum += expectedEuler[j];
      }
      errorEuler = 100 * Math.sqrt(errorEuler) / Math.sqrt(sum);

      //			System.out.println(errorEuler);

      totalEuler += errorEuler;
      totalTran += errorTran;
    }

    int N = numTrials - totalFail;

    System.out.printf(
        "%20s N = %d failed %.3f%% euler = %3f%% tran = %5f\n",
        name,
        target.getMinimumPoints(),
        (totalFail / (double) numTrials),
        (totalEuler / N),
        (totalTran / N));
  }
  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));
  }
Exemplo n.º 4
0
 @Override
 public boolean generate(List<Point2D3D> dataSet, Se3_F64 model) {
   return alg.process(dataSet, model);
 }