/**
   * 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;
  }
  /** Apply a rotation such that the epipole is equal to [f,0,1)\ */
  private SimpleMatrix rotateEpipole(Point3D_F64 epipole, int x0, int y0) {
    // compute rotation which will set
    // x * sin(theta) + y * cos(theta) = 0

    double x = epipole.x / epipole.z - x0;
    double y = epipole.y / epipole.z - y0;

    double theta = Math.atan2(-y, x);
    double c = Math.cos(theta);
    double s = Math.sin(theta);

    SimpleMatrix R = new SimpleMatrix(3, 3);
    R.setRow(0, 0, c, -s);
    R.setRow(1, 0, s, c);
    R.set(2, 2, 1);

    return R;
  }