package photogrammetry.util; import java.util.ArrayList; import java.util.List; import Jama.Matrix; import Jama.SingularValueDecomposition; /** * Performs estimation of rotation and translation between two views using the essential matrix. * * @author johannes */ public class RotationTranslationEstimator { private final Matrix t; private final Matrix r1; private final Matrix r2; /** * Construct a rotation & translation estimator for an essential matrix. Implemented according * to http://www.comp.nus.edu.sg/~cs4243/lecture/multiview.pdf, slides 52 to 54. * * @param e * the essential matrix */ public RotationTranslationEstimator(Matrix e) { final SingularValueDecomposition svd = new SingularValueDecomposition(e); System.out.println("U:"); System.out.println(MatrixUtils.matrixToString(svd.getU())); System.out.println("S:"); System.out.println(MatrixUtils.matrixToString(svd.getS())); System.out.println("V:"); System.out.println(MatrixUtils.matrixToString(svd.getV())); Matrix tt = new Matrix(3, 1); for (int i = 0; i < 3; i++) { tt.set(i, 0, svd.getU().get(i, 2)); } t = tt.times(1 / tt.normF()); Matrix RT = new Matrix(new double[][] { new double[] { 0, -1, 0 }, new double[] { 1, 0, 0 }, new double[] { 0, 0, 1 } }); List<Matrix> matrices = new ArrayList<>(4); matrices.add(svd.getU().times(RT).times(svd.getV().transpose())); matrices.add(svd.getU().times(RT).times(svd.getV().transpose()).times(-1)); RT = RT.transpose(); matrices.add(svd.getU().times(RT).times(svd.getV().transpose())); matrices.add(svd.getU().times(RT).times(svd.getV().transpose()).times(-1)); for (int i = 3; i >= 0; i--) { if (Math.abs(matrices.get(i).det() - 1) > 0.0001) { matrices.remove(i); } } r1 = matrices.get(0); r2 = matrices.get(1); } /** * Return the estimated direction of translation. * * @return the direction of translation */ public Matrix getT() { return t; } /** * Return the first possible rotation matrix. * * @return the first of two rotation matrices */ public Matrix getR1() { return r1; } /** * Return the second possible rotation matrix. * * @return the second of two rotation matrices */ public Matrix getR2() { return r2; } @Override public String toString() { return "Translation: \n" + MatrixUtils.matrixToString(t) + "R1: \n" + MatrixUtils.matrixToString(r1) + "R2: \n" + MatrixUtils.matrixToString(r2); } }