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

}