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) {
P = new SimpleMatrix(SimpleOperators.multiplyMatrixProd(
normalizationMatrices.get(projectionMatrices.indexOf(P))
.inverse(InversionType.INVERT_QR), P));
list.add(P);
}
projectionMatrices = new ArrayList<SimpleMatrix>(list);
}
/**
* extracts world points of shape S
*/
private void setWorldPoints() {
worldPoints = new ArrayList<PointND>();
for (int i = 0; i < S.getCols(); i++) {
SimpleVector col = S.getCol(i);
PointND p = new PointND(col.getElement(0)/col.getElement(3), col.getElement(1)/col.getElement(3),
col.getElement(2)/col.getElement(3));
worldPoints.add(p);
}
}
/**
*
* @return projectionMatrices
*/
public ArrayList<SimpleMatrix> getProjectionMatrices() {
return projectionMatrices;
}
/**
* returns specific projection matrix at frame no. slice
*
* @param slice
* @return projectionMatrices.get(slice)
*/
public SimpleMatrix getProjectionMatrix(int slice) {
return projectionMatrices.get(slice);
}
/**
*
* @return world points
*/
public ArrayList<PointND> getWorldPoints() {
return worldPoints;
}
/**
*
* @return all image points of all frames
*/
public ArrayList<ArrayList<PointND>> getImagePoints() {
return imagePoints;
}
/**
*
* @return number of frames
*/
public int getNumberOfFrames() {
return numberOfFrames;
}
/**
*
* @return number of points
*/
public int getNumberOfPoints() {
return numberOfPoints;
}
/**
*
* @param i
* @param j
* @return returns value of W at row = i and col = j
*/
public double getValueAt(int i, int j) {
return lambda.getElement(i / 3, j) * M.getElement(i, j);
}
/**
*
* @param i
* @param j
* @return returns lambda of W at row = i and col = j
*/
public double getLambda(int i, int j) {
return lambda.getElement(i / 3, j);
}
/**
* sets lambda
*
* @param i
* @param j
* @param value
*/
public void setLambda(int i, int j, double value) {
lambda.setElementValue(i / 3, j, value);
}
/**
*
* @param col
* @return single column of W
*/
public SimpleVector getCol(int col) {
SimpleVector ret = new SimpleVector(W.getRows());
for (int i = 0; i < W.getRows(); i++) {
ret.setElementValue(i, getValueAt(i, col));
}
return ret;
}
/**
*
* @param startRow
* @return row triplet containg startRow, startRow + 1 and startRow + 2
* linearized
*/
public SimpleVector getRowTriplet(int startRow) {
SimpleVector ret = new SimpleVector(W.getCols() * 3);
for (int i = 0; i < W.getCols(); i++) {
for (int j = startRow; j < startRow + 3; j++) {
ret.setElementValue(3 * i + j - startRow, getValueAt(j, i));
}
}
return ret;
}
/**
*
* @param col
* @return column of lambda
*/
public SimpleVector getLambdaCol(int col) {
SimpleVector ret = new SimpleVector(lambda.getRows());
for (int i = 0; i < lambda.getRows(); i++) {
ret.setElementValue(i, getValueAt(i, col));
}
return ret;
}
/**
*
* @param row
* @return row of lambda
*/
public SimpleVector getLambdaRow(int row) {
SimpleVector ret = new SimpleVector(lambda.getRows());
for (int i = 0; i < lambda.getCols(); i++) {
ret.setElementValue(i, getValueAt(row, i));
}
return ret;
}
private static PointND compute2Dfrom3D(PointND point3D, Projection pMatrix) {
// Compute coordinates in projection data.
SimpleVector homogeneousPoint = SimpleOperators.multiply(pMatrix
.computeP(), new SimpleVector(point3D.get(0), point3D.get(1),
point3D.get(2), 1.0));
// Do forward projection to 2D coordinates
double coordU = homogeneousPoint.getElement(0)
/ homogeneousPoint.getElement(2);
double coordV = homogeneousPoint.getElement(1)
/ homogeneousPoint.getElement(2);
return new PointND(coordU, coordV);
}
public static void main(String[] args) {
ArrayList<PointND> world = new ArrayList<PointND>();
world.add(new PointND(10, 20, 30));
world.add(new PointND(11, 19, 25));
world.add(new PointND(12, 18, 20));
world.add(new PointND(13, 17, 15));
world.add(new PointND(14, 16, 10));
world.add(new PointND(15, 15, 5));
world.add(new PointND(16, 14, 0));
world.add(new PointND(17, 13, -5));
CONRAD.setup();
ArrayList<ArrayList<PointND>> image = new ArrayList<ArrayList<PointND>>();
for (int i = 0; i < Configuration.getGlobalConfiguration()
.getGeometry().getProjectionMatrices().length; i++) {
ArrayList<PointND> list = new ArrayList<PointND>();
for (int j = 0; j < world.size(); j++) {
list.add(compute2Dfrom3D(world.get(j), Configuration
.getGlobalConfiguration().getGeometry()
.getProjectionMatrix(i)));
}
image.add(list);
}
Factorization test = new Factorization(image);
}
}