/**
* 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;
}
/**
* Computes a basis (the principle components) from the most dominant eigenvectors.
*
* @param numComponents Number of vectors it will use to describe the data. Typically much smaller
* than the number of elements in the input vector.
*/
public void computeBasis(int numComponents) {
if (numComponents > A.getNumCols())
throw new IllegalArgumentException("More components requested that the data's length.");
if (sampleIndex != A.getNumRows())
throw new IllegalArgumentException("Not all the data has been added");
if (numComponents > sampleIndex)
throw new IllegalArgumentException(
"More data needed to compute the desired number of components");
this.numComponents = numComponents;
// compute the mean of all the samples
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
mean[j] += A.get(i, j);
}
}
for (int j = 0; j < mean.length; j++) {
mean[j] /= A.getNumRows();
}
// subtract the mean from the original data
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < mean.length; j++) {
A.set(i, j, A.get(i, j) - mean[j]);
}
}
// Compute SVD and save time by not computing U
SingularValueDecomposition<DenseMatrix64F> svd =
DecompositionFactory.svd(A.numRows, A.numCols, false, true, false);
if (!svd.decompose(A)) throw new RuntimeException("SVD failed");
V_t = svd.getV(null, true);
DenseMatrix64F W = svd.getW(null);
// Singular values are in an arbitrary order initially
SingularOps.descendingOrder(null, false, W, V_t, true);
// strip off unneeded components and find the basis
V_t.reshape(numComponents, mean.length, true);
}
/**
* Parametrization for Bundle Adjustment with known calibration where the rotation matrix is encoded
* using {@link Rodrigues} coordinates.
*
* @author Peter Abeles
*/
public class CalibPoseAndPointRodriguesCodec implements ModelCodec<CalibratedPoseAndPoint> {
// number of camera views
int numViews;
// number of points in world coordinates
int numPoints;
// number of views with unknown extrinsic parameters
int numViewsUnknown;
// indicates if a view is known or not
boolean knownView[];
// storage
Rodrigues rotation = new Rodrigues();
DenseMatrix64F R = new DenseMatrix64F(3, 3);
// used to make sure the rotation matrix is in SO(3)
SingularValueDecomposition<DenseMatrix64F> svd =
DecompositionFactory.svd(3, 3, true, true, false);
/** Specify the number of views and points it can expected */
public void configure(int numViews, int numPoints, int numViewsUnknown, boolean[] knownView) {
if (numViews < knownView.length)
throw new IllegalArgumentException("Number of views is less than knownView length");
this.numViews = numViews;
this.numPoints = numPoints;
this.numViewsUnknown = numViewsUnknown;
this.knownView = knownView;
}
@Override
public void decode(double[] param, CalibratedPoseAndPoint outputModel) {
int paramIndex = 0;
// first decode the transformation
for (int i = 0; i < numViews; i++) {
// don't decode if it is already known
if (knownView[i]) continue;
Se3_F64 se = outputModel.getWorldToCamera(i);
rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]);
RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR());
Vector3D_F64 T = se.getT();
T.x = param[paramIndex++];
T.y = param[paramIndex++];
T.z = param[paramIndex++];
}
// now decode the points
for (int i = 0; i < numPoints; i++) {
Point3D_F64 p = outputModel.getPoint(i);
p.x = param[paramIndex++];
p.y = param[paramIndex++];
p.z = param[paramIndex++];
}
}
@Override
public void encode(CalibratedPoseAndPoint model, double[] param) {
int paramIndex = 0;
// first decode the transformation
for (int i = 0; i < numViews; i++) {
// don't encode if it is already known
if (knownView[i]) continue;
Se3_F64 se = model.getWorldToCamera(i);
// force the "rotation matrix" to be an exact rotation matrix
// otherwise Rodrigues will have issues with the noise
if (!svd.decompose(se.getR())) throw new RuntimeException("SVD failed");
DenseMatrix64F U = svd.getU(null, false);
DenseMatrix64F V = svd.getV(null, false);
CommonOps.multTransB(U, V, R);
// extract Rodrigues coordinates
RotationMatrixGenerator.matrixToRodrigues(R, rotation);
param[paramIndex++] = rotation.unitAxisRotation.x * rotation.theta;
param[paramIndex++] = rotation.unitAxisRotation.y * rotation.theta;
param[paramIndex++] = rotation.unitAxisRotation.z * rotation.theta;
Vector3D_F64 T = se.getT();
param[paramIndex++] = T.x;
param[paramIndex++] = T.y;
param[paramIndex++] = T.z;
}
for (int i = 0; i < numPoints; i++) {
Point3D_F64 p = model.getPoint(i);
param[paramIndex++] = p.x;
param[paramIndex++] = p.y;
param[paramIndex++] = p.z;
}
}
@Override
public int getParamLength() {
return numViewsUnknown * 6 + numPoints * 3;
}
}
public TrifocalExtractEpipoles() {
svd = DecompositionFactory.svd(3, 3, true, true, true);
svd = new SafeSvd(svd);
}
/**
* Creates a new solver targeted at the specified matrix size.
*
* @param maxRows The expected largest matrix it might have to process. Can be larger.
* @param maxCols The expected largest matrix it might have to process. Can be larger.
*/
public SolvePseudoInverseSvd(int maxRows, int maxCols) {
svd = DecompositionFactory.svd(maxRows, maxCols, true, true, true);
}
