gt: 			P = new SimpleMatrix(SimpleOperators.multiplyMatrixProd(
context: package edu.stanford.rsl.conrad.calibration;
  
  import java.util.ArrayList;
  
  import edu.stanford.rsl.conrad.geometry.Projection;
  import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
  import edu.stanford.rsl.conrad.numerics.DecompositionSVD;
  import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
  import edu.stanford.rsl.conrad.numerics.SimpleMatrix.InversionType;
  import edu.stanford.rsl.conrad.numerics.SimpleOperators;
  import edu.stanford.rsl.conrad.numerics.SimpleVector;
  import edu.stanford.rsl.conrad.utils.CONRAD;
  import edu.stanford.rsl.conrad.utils.Configuration;
  
  /**
   * Implements the perspective Factorization by Sturm in order to obtain
   * projection matrices. Still yields errors and results are not reliable.
   * 
   * @author Philipp Roser
   * 
   */
  public class Factorization {
  
  	/**
  	 * contains normalized image points
  	 */
  	private SimpleMatrix M;
  
  	/**
  	 * contains perspective depths
  	 */
  	private SimpleMatrix lambda;
  
  	/**
  	 * contains all matrices used for normalization
  	 */
  	private ArrayList<SimpleMatrix> normalizationMatrices;
  
  	/**
  	 * measurement matrix
  	 */
  	private SimpleMatrix W;
  
  	/**
  	 * matrix containing motion (projection matrices for each frame)
  	 */
  	private SimpleMatrix R;
  
  	/**
  	 * matrix containing shape (world points)
  	 */
  	private SimpleMatrix S;
  
  	/**
  	 * contains projection matrices for each frame
  	 */
  	private ArrayList<SimpleMatrix> projectionMatrices;
  
  	/**
  	 * contains world points
  	 */
  	private ArrayList<PointND> worldPoints;
  
  	/**
  	 * data set to be factorized. Note that each image point has to be visible
  	 * in each frame.
  	 */
  	private ArrayList<ArrayList<PointND>> imagePoints;
  
  	/**
  	 * formerly used to save normalized image points
  	 * 
  	 * @deprecated actually no use
  	 */
  	private ArrayList<ArrayList<PointND>> registeredPoints;
  
  	/**
  	 * number of points in each frame
  	 */
  	private int numberOfPoints;
  
  	/**
  	 * number of frames
  	 */
  	private int numberOfFrames;
  
  	/**
  	 * Executes the complete perpective Factorization
  	 * 
  	 * @param points
  	 *            , each point has to be visible in each frame.
  	 */
  	public Factorization(ArrayList<ArrayList<PointND>> points) {
  		imagePoints = points;
  		numberOfFrames = points.size();
  		numberOfPoints = points.get(0).size();
  
  		initLambda();
  		initM();
  		computeW();
  		estimateLambda();
  		denormalizePMatrices();
  	}
  
  	/**
  	 * initializes perspective depths with 1
  	 */
  	private void initLambda() {
  		lambda = new SimpleMatrix(this.numberOfFrames, this.numberOfPoints);
  		lambda.ones();
  	}
  
  	/**
  	 * normalized points and builds M. rank = 4 is enforced.
  	 */
  	private void initM() {
  		normalizationMatrices = new ArrayList<SimpleMatrix>();
  		M = new SimpleMatrix(3 * numberOfFrames, numberOfPoints);
  		for (int i = 0; i < 3 * numberOfFrames; i += 3) {
  			SimpleMatrix T = estimateNormalization(imagePoints.get(i / 3));
  			for (int j = 0; j < numberOfPoints; j++) {
  				M.setElementValue(i, j, imagePoints.get(i / 3).get(j).get(0)
  						* T.getElement(0, 0) + T.getElement(0, 2));
  				M.setElementValue(i + 1, j, imagePoints.get(i / 3).get(j)
  						.get(1)
  						* T.getElement(1, 1) + T.getElement(1, 2));
  				M.setElementValue(i + 2, j, 1.0);
  			}
  		}
  		DecompositionSVD svd = new DecompositionSVD(M);
  		SimpleMatrix newS = new SimpleMatrix(svd.getS().getRows(), svd.getS()
  				.getCols());
  		newS.zeros();
  		newS.setElementValue(0, 0, svd.getS().getElement(0, 0));
  		newS.setElementValue(1, 1, svd.getS().getElement(1, 1));
  		newS.setElementValue(2, 2, svd.getS().getElement(2, 2));
  		newS.setElementValue(3, 3, svd.getS().getElement(3, 3));
  		M = new SimpleMatrix(SimpleOperators.multiplyMatrixProd(SimpleOperators
  				.multiplyMatrixProd(svd.getU(), newS), svd.getV().transposed()));
  	}
  
  	/**
  	 * computes W out of M and lambda. rank = 4 is enforced.
  	 */
  	private void computeW() {
  		W = new SimpleMatrix(M.getRows(), M.getCols());
  		for (int i = 0; i < M.getRows(); i++) {
  			for (int j = 0; j < M.getCols(); j++) {
  				W.setElementValue(i, j, getValueAt(i, j));
  			}
  		}
  		DecompositionSVD svd = new DecompositionSVD(W);
  		SimpleMatrix newS = new SimpleMatrix(svd.getS().getRows(), svd.getS()
  				.getCols());
  		newS.zeros();
  		newS.setElementValue(0, 0, svd.getS().getElement(0, 0));
  		newS.setElementValue(1, 1, svd.getS().getElement(1, 1));
  		newS.setElementValue(2, 2, svd.getS().getElement(2, 2));
  		newS.setElementValue(3, 3, svd.getS().getElement(3, 3));
  		W = new SimpleMatrix(SimpleOperators.multiplyMatrixProd(SimpleOperators
  				.multiplyMatrixProd(svd.getU(), newS), svd.getV().transposed()));
  	}
  
  	/**
  	 * formerly used to normalize points
  	 * 
  	 * @deprecated not used any longer
  	 */
  	private void registerPoints() {
  		this.registeredPoints = new ArrayList<ArrayList<PointND>>();
  		for (ArrayList<PointND> list : this.imagePoints) {
  			double sumX = 0;
  			double sumY = 0;
  			for (PointND p : list) {
  				sumX += p.get(0);
  				sumY += p.get(1);
  			}
  			ArrayList<PointND> registered = new ArrayList<PointND>();
  			PointND centroid = new PointND(sumX / list.size(), sumY
  					/ list.size());
  			for (PointND p : list) {
  				PointND registeredP = new PointND(p.get(0) - centroid.get(0),
  						p.get(1) - centroid.get(1));
  				registered.add(registeredP);
  			}
  			this.registeredPoints.add(registered);
  		}
  	}
  
  	/**
  	 * formerly used to normalize points
  	 * 
  	 * @deprecated not used any longer
  	 */
  	private void normalizePoints() {
  		for (ArrayList<PointND> list : this.registeredPoints) {
  			int meanDistance = 0;
  			for (PointND p : list) {
  				meanDistance += p.getAbstractVector().normL2();
  			}
  			meanDistance = meanDistance / list.size();
  			for (PointND p : list) {
  				p.set(0, p.get(0) * Math.sqrt(2.0) / meanDistance);
  				p.set(1, p.get(1) * Math.sqrt(2.0) / meanDistance);
  			}
  		}
  	}
  
  	/**
  	 * estimates centroid of list
  	 * 
  	 * @param list
  	 *            , containing all image points of one frame
  	 * @return centroid of the data set
  	 */
  	private PointND estimateCentroid2D(ArrayList<PointND> list) {
  		PointND centroid = new PointND(0, 0);
  		for (PointND p : list) {
  			centroid.set(0, centroid.get(0) + p.get(0) / list.size());
  			centroid.set(1, centroid.get(1) + p.get(1) / list.size());
  		}
  		return centroid;
  	}
  
  	/**
  	 * estimates the scale for uniform scaling
  	 * 
  	 * @param centroid
  	 *            of list
  	 * @param list
  	 *            of image points
  	 * @return scale
  	 */
  	private double estimateScale(PointND centroid, ArrayList<PointND> list) {
  		double scale = 0.0;
  		for (PointND p : list) {
  			scale += p.euclideanDistance(centroid);
  		}
  		scale = Math.sqrt(2.0) / scale;
  		return scale;
  	}
  
  	/**
  	 * estimates the similarity transform matrix T
  	 * 
  	 * @param list
  	 *            , containing image points of one frame
  	 * @return similarity transform T
  	 */
  	private SimpleMatrix estimateNormalization(ArrayList<PointND> list) {
  		// estimate centroid and scale
  		PointND centroid = estimateCentroid2D(list);
  		double scale = estimateScale(centroid, list);
  		// build transformation matrix T
  		SimpleMatrix T = new SimpleMatrix(3, 3);
  		// first row
  		T.setElementValue(0, 0, scale);
  		T.setElementValue(0, 1, 0);
  		T.setElementValue(0, 2, -centroid.get(0) * scale);
  		// second row
  		T.setElementValue(1, 0, 0);
  		T.setElementValue(1, 1, scale);
  		T.setElementValue(1, 2, -centroid.get(1) * scale);
  		// third row
  		T.setElementValue(2, 0, 0);
  		T.setElementValue(2, 1, 0);
  		T.setElementValue(2, 2, 1);
  		normalizationMatrices.add(T);
  		return T;
  	}
  
  	/**
  	 * estimates the perspective depths iteratively
  	 */
  	private void estimateLambda() {
  
  		double diff = 100.0;
  
  		while (diff > CONRAD.DOUBLE_EPSILON) {
  
  			diff = 0;
  
  			for (int i = 0; i < W.getCols(); i++) {
  				double norm = getCol(i).normL2();
  				for (int j = 0; j < W.getRows(); j += 3) {
  					setLambda(j / 3, i, getLambda(j / 3, i) / (norm));
  				}
  			}
  
  			for (int i = 0; i < W.getRows(); i += 3) {
  				double norm = getRowTriplet(i).normL2();
  				for (int j = 0; j < W.getCols(); j++) {
  					lambda.setElementValue(i / 3, j,
  							lambda.getElement(i / 3, j) / (norm));
  				}
  			}
  
  			/*
  			 * for (int i = 0; i < lambda.getCols(); i++) { double norm =
  			 * getLambdaCol(i).normL2(); for (int j = 0; j < lambda.getRows();
  			 * j++) { setLambda(j, i, getLambda(j, i) / Math.sqrt(norm)); } }
  			 * 
  			 * for (int i = 0; i < lambda.getRows(); i++) { double norm =
  			 * getLambdaRow(i).normL2(); for (int j = 0; j < lambda.getCols();
  			 * j++) { lambda.setElementValue(i / 3, j, lambda.getElement(i / 3,
  			 * j) / Math.sqrt(norm)); } }
  			 */
  
  			SimpleMatrix oldW = new SimpleMatrix(W);
  			computeW();
  			decomposeW();
  			setProjectionMatrices();
  			setWorldPoints();
  
  			/*
  			 * for (int i = 0; i < projectionMatrices.size(); i++) { for (int j
  			 * = 0; j < worldPoints.size(); j++) { SimpleVector p3 =
  			 * projectionMatrices.get(i).getSubRow(2, 0, 4); SimpleVector wj =
  			 * worldPoints.get(j).getAbstractVector(); setLambda(i, j,
  			 * SimpleOperators.multiplyInnerProd(p3, wj)); } }
  			 * 
  			 * computeW(); decomposeW(); setProjectionMatrices();
  			 * setWorldPoints();
  			 */
  
  			SimpleMatrix delta = SimpleOperators.subtract(W, oldW);
  			DecompositionSVD svdDelta = new DecompositionSVD(delta);
  			DecompositionSVD svdM = new DecompositionSVD(oldW);
  			diff = svdDelta.norm2() / svdM.norm2();
  		}
  
  	}
  
  	/**
  	 * factorizes the measurement matrix W to motion R and shape S.
  	 */
  	private void decomposeW() {
  		DecompositionSVD svd = new DecompositionSVD(W);
  		R = SimpleOperators.multiplyMatrixProd(svd.getU(), svd.getS());
  		S = svd.getV();
  	}
  
  	/**
  	 * extracts projection matrices of R
  	 */
  	private void setProjectionMatrices() {
  		projectionMatrices = new ArrayList<SimpleMatrix>();
  		for (int i = 0; i < R.getRows(); i += 3) {
  			SimpleMatrix P = R.getSubMatrix(i, 0, 3, 4);
  			projectionMatrices.add(P);
  		}
  	}
  
  	/**
  	 * applies the inverse of T
  	 */
  	private void denormalizePMatrices() {
  		ArrayList<SimpleMatrix> list = new ArrayList<SimpleMatrix>();
  		for (SimpleMatrix P : projectionMatrices) {
  