コード例 #1
0
  /**
   * Finds a rotation matrix which is the optimal approximation to an arbitrary 3 by 3 matrix.
   * Optimality is specified by the equation below:<br>
   * <br>
   * min ||R-Q||<sup>2</sup><sub>F</sub><br>
   * R<br>
   * where R is the rotation matrix and Q is the matrix being approximated.
   *
   * <p>
   *
   * <p>The technique used is based on SVD and is described in Appendix C of "A Flexible New
   * Technique for Camera Calibration" Technical Report, updated 2002.
   *
   * <p>
   *
   * <p>Both origin and R can be the same instance.
   *
   * @param orig Input approximate rotation matrix. Not modified.
   * @param R (Optional) Storage for the approximated rotation matrix. Modified.
   * @return Rotation matrix
   */
  public static DenseMatrix64F approximateRotationMatrix(DenseMatrix64F orig, DenseMatrix64F R) {
    R = checkDeclare3x3(R);

    SingularValueDecomposition<DenseMatrix64F> svd =
        DecompositionFactory.svd(orig.numRows, orig.numCols, true, true, false);

    if (!svd.decompose(orig)) throw new RuntimeException("SVD Failed");

    CommonOps.mult(svd.getU(null, false), svd.getV(null, true), R);

    // svd does not guarantee that U anv V have positive determinants.
    float det = (float) CommonOps.det(R);

    if (det < 0) CommonOps.scale(-1, R);

    return R;
  }
コード例 #2
0
  @Override
  public boolean setA(DenseMatrix64F A) {
    pinv.reshape(A.numCols, A.numRows, false);

    if (!svd.decompose(A)) return false;

    DenseMatrix64F U_t = svd.getU(null, true);
    DenseMatrix64F V = svd.getV(null, false);
    double[] S = svd.getSingularValues();
    int N = Math.min(A.numRows, A.numCols);

    // compute the threshold for singular values which are to be zeroed
    double maxSingular = 0;
    for (int i = 0; i < N; i++) {
      if (S[i] > maxSingular) maxSingular = S[i];
    }

    double tau = threshold * Math.max(A.numCols, A.numRows) * maxSingular;

    // computer the pseudo inverse of A
    for (int i = 0; i < N; i++) {
      double s = S[i];
      if (s < tau) S[i] = 0;
      else S[i] = 1.0 / S[i];
    }

    // V*W
    for (int i = 0; i < V.numRows; i++) {
      int index = i * V.numCols;
      for (int j = 0; j < V.numCols; j++) {
        V.data[index++] *= S[j];
      }
    }

    // V*W*U^T
    CommonOps.mult(V, U_t, pinv);

    return true;
  }