/**
* 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;
}
@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;
}
@Override
public boolean modifiesA() {
return svd.inputModified();
}