/**
   * Computes inverse coefficients
   *
   * @param border
   * @param forward Forward coefficients.
   * @param inverse Inverse used in the inner portion of the data stream.
   * @return
   */
  private static WlBorderCoef<WlCoef_F32> computeBorderCoefficients(
      BorderIndex1D border, WlCoef_F32 forward, WlCoef_F32 inverse) {
    int N = Math.max(forward.getScalingLength(), forward.getWaveletLength());
    N += N % 2;
    N *= 2;
    border.setLength(N);

    // Because the wavelet transform is a linear invertible system the inverse coefficients
    // can be found by creating a matrix and inverting the matrix.  Boundary conditions are then
    // extracted from this inverted matrix.
    DenseMatrix64F A = new DenseMatrix64F(N, N);
    for (int i = 0; i < N; i += 2) {

      for (int j = 0; j < forward.scaling.length; j++) {
        int index = border.getIndex(j + i + forward.offsetScaling);
        A.add(i, index, forward.scaling[j]);
      }

      for (int j = 0; j < forward.wavelet.length; j++) {
        int index = border.getIndex(j + i + forward.offsetWavelet);
        A.add(i + 1, index, forward.wavelet[j]);
      }
    }

    LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(N);
    if (!solver.setA(A) || solver.quality() < 1e-5) {
      throw new IllegalArgumentException("Can't invert matrix");
    }

    DenseMatrix64F A_inv = new DenseMatrix64F(N, N);
    solver.invert(A_inv);

    int numBorder = UtilWavelet.borderForwardLower(inverse) / 2;

    WlBorderCoefFixed<WlCoef_F32> ret = new WlBorderCoefFixed<>(numBorder, numBorder + 1);
    ret.setInnerCoef(inverse);

    // add the lower coefficients first
    for (int i = 0; i < ret.getLowerLength(); i++) {
      computeLowerCoef(inverse, A_inv, ret, i * 2);
    }

    // add upper coefficients
    for (int i = 0; i < ret.getUpperLength(); i++) {
      computeUpperCoef(inverse, N, A_inv, ret, i * 2);
    }

    return ret;
  }
示例#2
0
  /**
   * Computes the most dominant eigen vector of A using an inverted shifted matrix. The inverted
   * shifted matrix is defined as <b>B = (A - &alpha;I)<sup>-1</sup></b> and can converge faster if
   * &alpha; is chosen wisely.
   *
   * @param A An invertible square matrix matrix.
   * @param alpha Shifting factor.
   * @return If it converged or not.
   */
  public boolean computeShiftInvert(DenseMatrix64F A, double alpha) {
    initPower(A);

    LinearSolver solver = LinearSolverFactory.linear(A.numCols);

    SpecializedOps.addIdentity(A, B, -alpha);
    solver.setA(B);

    boolean converged = false;

    for (int i = 0; i < maxIterations && !converged; i++) {
      solver.solve(q0, q1);
      double s = NormOps.normPInf(q1);
      CommonOps.divide(q1, s, q2);

      converged = checkConverged(A);
    }

    return converged;
  }
/**
 * Computes the Sampson distance residual for a set of observations given a homography matrix. For
 * use in least-squares non-linear optimization algorithms. The full 9 elements of the 3x3 matrix
 * are used to parameterize. This has an extra redundant parameter, but is much simpler and should
 * not affect the final result.
 *
 * <p>R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge
 * 2003
 *
 * @author Peter Abeles
 */
public class HomographyResidualSampson
    implements ModelObservationResidualN<DenseMatrix64F, AssociatedPair> {

  DenseMatrix64F H;
  Point2D_F64 temp = new Point2D_F64();

  DenseMatrix64F J = new DenseMatrix64F(2, 4);
  DenseMatrix64F JJ = new DenseMatrix64F(2, 2);
  DenseMatrix64F e = new DenseMatrix64F(2, 1);
  DenseMatrix64F x = new DenseMatrix64F(2, 1);
  DenseMatrix64F error = new DenseMatrix64F(4, 1);

  LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(2);

  @Override
  public void setModel(DenseMatrix64F F) {
    this.H = F;
  }

  @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;
  }

  /** x2 = H*x1 */
  public double error1(double x1, double y1, double x2, double y2) {
    double ret;

    ret = -(x1 * H.get(1, 0) + y1 * H.get(1, 1) + H.get(1, 2));
    ret += y2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));

    return ret;
  }

  public double error2(double x1, double y1, double x2, double y2) {
    double ret;

    ret = (x1 * H.get(0, 0) + y1 * H.get(0, 1) + H.get(0, 2));
    ret -= x2 * (x1 * H.get(2, 0) + y1 * H.get(2, 1) + H.get(2, 2));

    return ret;
  }

  public void computeJacobian(Point2D_F64 x1, Point2D_F64 x2) {
    J.data[0] = -H.get(1, 0) + x2.y * H.get(2, 0);
    J.data[1] = -H.get(1, 1) + x2.y * H.get(2, 1);
    J.data[2] = 0;
    J.data[3] = x1.x * H.get(2, 0) + x1.y * H.get(2, 1) + H.get(2, 2);

    J.data[4] = H.get(0, 0) - x2.x * H.get(2, 0);
    J.data[5] = H.get(0, 1) - x2.x * H.get(2, 1);
    J.data[6] = -J.data[3];
    J.data[7] = 0;
  }

  @Override
  public int getN() {
    return 4;
  }
}