public void init(int N, boolean isFundamental) {
    // define the camera's motion
    worldToCamera = new Se3_F64();
    worldToCamera
        .getR()
        .set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    worldToCamera.getT().set(0.1, -0.1, 0.01);

    // randomly generate points in space
    worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);

    // transform points into second camera's reference frame
    pairs = new ArrayList<AssociatedPair>();
    currentObs = new ArrayList<Point2D_F64>();

    for (Point3D_F64 p1 : worldPts) {
      Point3D_F64 p2 = SePointOps_F64.transform(worldToCamera, p1, null);

      AssociatedPair pair = new AssociatedPair();
      pair.p1.set(p1.x / p1.z, p1.y / p1.z);
      pair.p2.set(p2.x / p2.z, p2.y / p2.z);
      pairs.add(pair);

      if (isFundamental) {
        GeometryMath_F64.mult(K, pair.p1, pair.p1);
        GeometryMath_F64.mult(K, pair.p2, pair.p2);
      }

      currentObs.add(pair.p2);
    }
  }
  /**
   * Finds the values of a,b,c which minimize
   *
   * <p>sum (a*x(+)_i + b*y(+)_i + c - x(-)_i)^2
   *
   * <p>See page 306
   *
   * @return Affine transform
   */
  private SimpleMatrix computeAffineH(
      List<AssociatedPair> observations, DenseMatrix64F H, DenseMatrix64F Hzero) {
    SimpleMatrix A = new SimpleMatrix(observations.size(), 3);
    SimpleMatrix b = new SimpleMatrix(A.numRows(), 1);

    Point2D_F64 c = new Point2D_F64();
    Point2D_F64 k = new Point2D_F64();

    for (int i = 0; i < observations.size(); i++) {
      AssociatedPair a = observations.get(i);

      GeometryMath_F64.mult(Hzero, a.p1, k);
      GeometryMath_F64.mult(H, a.p2, c);

      A.setRow(i, 0, k.x, k.y, 1);
      b.set(i, 0, c.x);
    }

    SimpleMatrix x = A.solve(b);

    SimpleMatrix Ha = SimpleMatrix.identity(3);
    Ha.setRow(0, 0, x.getMatrix().data);

    return Ha;
  }
  /** 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);
    }
  }
  @Override
  public int computeResiduals(AssociatedPair p, double[] residuals, int index) {

    GeometryMath_F64.mult(H, p.p1, temp);

    double top1 = error1(p.p1.x, p.p1.y, p.p2.x, p.p2.y);
    double top2 = error2(p.p1.x, p.p1.y, p.p2.x, p.p2.y);

    computeJacobian(p.p1, p.p2);
    // JJ = J*J'
    CommonOps.multTransB(J, J, JJ);

    // solve JJ'*x = -e
    e.data[0] = -top1;
    e.data[1] = -top2;

    if (solver.setA(JJ)) {
      solver.solve(e, x);
      // -J'(J*J')^-1*e
      CommonOps.multTransA(J, x, error);
      residuals[index++] = error.data[0];
      residuals[index++] = error.data[1];
      residuals[index++] = error.data[2];
      residuals[index++] = error.data[3];
    } else {
      residuals[index++] = 0;
      residuals[index++] = 0;
      residuals[index++] = 0;
      residuals[index++] = 0;
    }

    return index;
  }
  public static Point3D_F64 createPt(Sphere3D_F64 sphere, double phi, double theta) {
    Point3D_F64 p = new Point3D_F64();
    p.set(0, 0, sphere.radius);

    Rodrigues_F64 rodX = new Rodrigues_F64(phi, new Vector3D_F64(1, 0, 0));
    DenseMatrix64F rotX = ConvertRotation3D_F64.rodriguesToMatrix(rodX, null);
    Rodrigues_F64 rodZ = new Rodrigues_F64(theta, new Vector3D_F64(0, 0, 1));
    DenseMatrix64F rotZ = ConvertRotation3D_F64.rodriguesToMatrix(rodZ, null);

    GeometryMath_F64.mult(rotX, p, p);
    GeometryMath_F64.mult(rotZ, p, p);
    p.x += sphere.center.x;
    p.y += sphere.center.y;
    p.z += sphere.center.z;

    return p;
  }
  @Override
  public double computeResidual(AssociatedPair observation) {
    double bottom = 0;

    GeometryMath_F64.mult(F, observation.keyLoc, temp);
    bottom += temp.x * temp.x + temp.y * temp.y;

    GeometryMath_F64.multTran(F, observation.currLoc, temp);
    bottom += temp.x * temp.x + temp.y * temp.y;

    bottom = Math.sqrt(bottom);

    if (bottom <= UtilEjml.EPS) {
      return Double.MAX_VALUE;
    } else {
      GeometryMath_F64.multTran(F, observation.currLoc, temp);

      return (temp.x * observation.keyLoc.x + temp.y * observation.keyLoc.y + temp.z) / bottom;
    }
  }
  @Override
  public boolean generate(List<PointVectorNN> dataSet, Cylinder3D_F64 output) {

    PointVectorNN pa = dataSet.get(0);
    PointVectorNN pb = dataSet.get(1);
    PointVectorNN pc = dataSet.get(2);

    lineA.p = pa.p;
    lineA.slope = pa.normal;

    lineB.p = pb.p;
    lineB.slope = pb.normal;

    // With perfect data, the closest point between the two lines defined by a point and its normal
    // will lie on the axis of the cylinder
    ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p);
    // The cylinder's axis will be along the cross product of the two normal vectors
    GeometryMath_F64.cross(pa.normal, pb.normal, output.line.slope);

    // take the normal of the slope so that it can detect if it has a value of zero, which happens
    // if the
    // two input vectors are the same
    double n = output.line.slope.norm();
    // n should actually always be one since the point normals are normalized to one.
    if (n < 1e-8) return false;
    output.line.slope.x /= n;
    output.line.slope.y /= n;
    output.line.slope.z /= n;

    // set the radius to be the average distance point is from the cylinder's axis
    double ra = Distance3D_F64.distance(output.line, pa.p);
    double rb = Distance3D_F64.distance(output.line, pb.p);
    double rc = Distance3D_F64.distance(output.line, pc.p);

    output.radius = (ra + rb) / 2.0;

    // sanity check using the model parameters
    if (!check.valid(output)) return false;

    // sanity check the model using points
    return checkModel(output, pc, ra, rb, rc);
  }