コード例 #1
0
  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);
    }
  }
コード例 #2
0
  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));
  }
コード例 #3
0
  /** Give it a grid and see if it computed a legitimate homography */
  @Test
  public void basicTest() {
    // create a grid an apply an arbitrary transform to it
    PlanarCalibrationTarget config = GenericCalibrationGrid.createStandardConfig();

    DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.02, -0.05, 0.01, null);
    Vector3D_F64 T = new Vector3D_F64(0, 0, -1000);
    Se3_F64 motion = new Se3_F64(R, T);

    List<Point2D_F64> observations = GenericCalibrationGrid.observations(motion, config);

    // compute the homography
    Zhang99ComputeTargetHomography alg = new Zhang99ComputeTargetHomography(config);

    assertTrue(alg.computeHomography(observations));

    DenseMatrix64F H = alg.getHomography();

    // test this homography property: x2 = H*x1
    List<Point2D_F64> gridPoints = config.points;
    for (int i = 0; i < observations.size(); i++) {
      Point2D_F64 a = GeometryMath_F64.mult(H, gridPoints.get(i), new Point2D_F64());

      double diff = a.distance(observations.get(i));
      assertEquals(0, diff, 1e-8);
    }
  }
コード例 #4
0
  /** The two sets of points match perfectly and with a little bit of noise */
  @Test
  public void test3D() {
    for (double noise = 0; noise <= 0.01; noise += 0.01) {
      // can only correct small changes
      DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.01, -0.002, 0.03, null);
      Vector3D_F64 T = new Vector3D_F64(0.02, 0.03, 0.01);

      Se3_F64 tran = new Se3_F64(R, T);

      List<Point3D_F64> srcPts = UtilPoint3D_F64.random(-10, 10, 30, rand);
      List<Point3D_F64> srcOrig = UtilPoint3D_F64.copy(srcPts);
      List<Point3D_F64> modelPts = new ArrayList<Point3D_F64>();
      for (Point3D_F64 p : srcPts) {
        modelPts.add(SePointOps_F64.transform(tran, p, null));
      }

      // add noise
      UtilPoint3D_F64.noiseNormal(modelPts, noise, rand);

      PointModel<Point3D_F64> model = new PointModel<Point3D_F64>(modelPts);
      StoppingCondition stop = new StoppingCondition(10, 0.1 * noise / srcPts.size() + 1e-8);
      IterativeClosestPoint<Se3_F64, Point3D_F64> alg =
          new IterativeClosestPoint<Se3_F64, Point3D_F64>(stop, new MotionSe3PointSVD_F64());
      alg.setModel(model);

      alg.process(srcPts);

      Se3_F64 foundTran = alg.getPointsToModel();

      checkTransform(srcOrig, modelPts, foundTran, noise * 10 + 1e-8);
    }
  }
コード例 #5
0
  @Override
  public void decode(double[] param, CalibratedPoseAndPoint outputModel) {

    int paramIndex = 0;

    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
      // don't decode if it is already known
      if (knownView[i]) continue;

      Se3_F64 se = outputModel.getWorldToCamera(i);

      rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]);

      RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR());

      Vector3D_F64 T = se.getT();
      T.x = param[paramIndex++];
      T.y = param[paramIndex++];
      T.z = param[paramIndex++];
    }

    // now decode the points
    for (int i = 0; i < numPoints; i++) {
      Point3D_F64 p = outputModel.getPoint(i);
      p.x = param[paramIndex++];
      p.y = param[paramIndex++];
      p.z = param[paramIndex++];
    }
  }
コード例 #6
0
  @Override
  public void encode(CalibratedPoseAndPoint model, double[] param) {
    int paramIndex = 0;

    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
      // don't encode if it is already known
      if (knownView[i]) continue;

      Se3_F64 se = model.getWorldToCamera(i);

      // force the "rotation matrix" to be an exact rotation matrix
      // otherwise Rodrigues will have issues with the noise
      if (!svd.decompose(se.getR())) throw new RuntimeException("SVD failed");

      DenseMatrix64F U = svd.getU(null, false);
      DenseMatrix64F V = svd.getV(null, false);

      CommonOps.multTransB(U, V, R);

      // extract Rodrigues coordinates
      RotationMatrixGenerator.matrixToRodrigues(R, rotation);

      param[paramIndex++] = rotation.unitAxisRotation.x * rotation.theta;
      param[paramIndex++] = rotation.unitAxisRotation.y * rotation.theta;
      param[paramIndex++] = rotation.unitAxisRotation.z * rotation.theta;

      Vector3D_F64 T = se.getT();

      param[paramIndex++] = T.x;
      param[paramIndex++] = T.y;
      param[paramIndex++] = T.z;
    }

    for (int i = 0; i < numPoints; i++) {
      Point3D_F64 p = model.getPoint(i);

      param[paramIndex++] = p.x;
      param[paramIndex++] = p.y;
      param[paramIndex++] = p.z;
    }
  }