package photogrammetry.util;
import java.util.Collection;
import Jama.Matrix;
import Jama.SingularValueDecomposition;
import photogrammetry.util.model.Feature;
import photogrammetry.util.model.Point2d;
import photogrammetry.util.model.SceneView;
/**
* Estimates the essential matrix.
*
* @author johannes
*/
public class EssentialMatrixEstimator {
/**
* Callback for debugging purposes.
*
* @author johannes
*/
public interface EssentialMatrixEstimatorCallback {
/**
* Called when the "A" matrix has been calculated.
*
* @param a the "A" matrix
*/
public void acceptA(Matrix a);
/**
* Called when the "e" vector has been calculated.
*
* @param e the "e" vector
*/
public void acceptEVector(Matrix e);
/**
* Called when the initial (non-refined) e matrix has been calculated.
*
* @param e Essential matrix
*/
public void acceptE(Matrix e);
/**
* Called when the homogenized image points have been calculated.
*
* @param index index of the view. 0 or 1.
* @param imgpts the homogenized image points
*/
public void acceptImagePoints(int index, Matrix imgpts);
}
private static final EssentialMatrixEstimatorCallback NULL_CALLBACK = new EssentialMatrixEstimatorCallback() {
@Override
public void acceptEVector(Matrix e) {
}
@Override
public void acceptE(Matrix e) {
}
@Override
public void acceptA(Matrix m) {
}
@Override
public void acceptImagePoints(int index, Matrix imgpts) {
}
};
private final Collection<Feature> commonFeatures;
private final SceneView image1;
private final SceneView image2;
private Matrix essentialMatrix, refinedEssentialMatrix;
private final EssentialMatrixEstimatorCallback callback;
private double error, refinedError;
/**
* Construct a new essential matrix estimator for two images.
*
* @param im1 the first view
* @param im2 the second view
* @param c the camera used in both views
*/
public EssentialMatrixEstimator(SceneView im1, SceneView im2, Camera c) {
this(im1, im2, c, null);
}
/**
* Construct a new essential matrix estimator for two images with a callback
* object.
*
* @param im1 the first view
* @param im2 the second view
* @param c the camera used in both views
* @param callback the callback object (can be null)
*/
public EssentialMatrixEstimator(SceneView im1, SceneView im2, Camera c,
EssentialMatrixEstimatorCallback callback) {
image1 = im1;
image2 = im2;
commonFeatures = image1.getCommonFeatures(image2);
this.callback = callback == null ? NULL_CALLBACK : callback;
estimateEssentialMatrix(c);
}
/**
* Gets homogenized undistorted image points.
*
* @param c the camera to use for undistorting
* @param img the image
* @return a 3x(commonFeatures.size()) Matrix
*/
private Matrix getHomogenizedPoints(Camera c, SceneView img) {
Matrix result = new Matrix(3, commonFeatures.size());
int i = 0;
for (Feature f : commonFeatures) {
Point2d p = c.undistort(img.getLocationInView(f));
result.set(0, i, p.x);
result.set(1, i, p.y);
result.set(2, i, 1);
i++;
}
return result;
}
/**
* Normalizes image points.
*
* @param points the points to normalize
* @param kInv the inverse of the camera matrix
*/
private void normalize(Matrix points, Matrix kInv) {
Matrix d = new Matrix(3, 1);
for (int i = 0; i < points.getColumnDimension(); i++) {
for (int j = 0; j < 3; j++) {
d.set(j, 0, points.get(j, i));
}
points.setMatrix(0, 2, i, i, kInv.times(d));
}
}
/**
* Create the "A" matrix
*
* @param points1 normalized points from first view
* @param points2 normalized points from second view
* @return matrix A (see slides)
*/
private Matrix buildA(Matrix points1, Matrix points2) {
Matrix result = new Matrix(points1.getColumnDimension(), 9);
for (int i = 0; i < result.getRowDimension(); i++) {
double x1i = points1.get(0, i);
double x2i = points2.get(0, i);
double y1i = points1.get(1, i);
double y2i = points2.get(1, i);
double[] row = new double[]{x1i * x2i, y1i * x2i, x2i, x1i * y2i, y1i * y2i, y2i,
x1i, y1i, 1};
for (int col = 0; col < 9; col++) {
result.set(i, col, row[col]);
}
}
return result;
}
/**
* Compute the error � (x2^T * E * x1)^2
*
* @param e essential matrix
* @param imgpts1 3xn matrix: (xi, yi, 1)
* @param imgpts2 3xn matrix: (xi, yi, 1)
* @return the error
*/
private double getError(Matrix e, Matrix imgpts1, Matrix imgpts2) {
double val = 0;
for (int i = 0; i < imgpts1.getColumnDimension(); i++) {
Matrix pt2 = imgpts2.getMatrix(0, 2, i, i).transpose();
Matrix pt1 = imgpts1.getMatrix(0, 2, i, i);
Matrix pt2timesE = pt2.times(e);
val += Math.pow(pt2timesE.times(pt1).get(0, 0), 2);
}
return val;
}
/**
* Estimate essential matrix
*
* @param c the camera used in both views
*/
private void estimateEssentialMatrix(Camera c) {
final Matrix imgpts2 = getHomogenizedPoints(c, image2);
final Matrix imgpts1 = getHomogenizedPoints(c, image1);
final Matrix kInv = c.getInvIntrinsics();
essentialMatrix = new Matrix(3, 3);
// normalize image points
normalize(imgpts1, kInv);
normalize(imgpts2, kInv);
callback.acceptImagePoints(0, imgpts1);
callback.acceptImagePoints(1, imgpts2);
final Matrix a = buildA(imgpts1, imgpts2);
callback.acceptA(a);
final Matrix ata = a.transpose().times(a);
final Pair<double[], Matrix[]> eigs = MatrixUtils.eig(ata);
// find evector
double minEval = Math.abs(eigs.a[0]);
Matrix evector = eigs.b[0];
callback.acceptEVector(evector);
for (int i = 1; i < eigs.a.length; i++) {
if (minEval > Math.abs(eigs.a[i])) {
minEval = Math.abs(eigs.a[i]);
evector = eigs.b[i];
}
}
for (int i = 0; i < 9; i++) {
essentialMatrix.set(i / 3, i % 3, evector.get(i, 0));
}
callback.acceptE(essentialMatrix);
error = getError(essentialMatrix, imgpts1, imgpts2);
SingularValueDecomposition svd = new SingularValueDecomposition(essentialMatrix);
Matrix s = svd.getS();
System.out.println("S:\n" + MatrixUtils.matrixToString(s));
s.set(2, 2, 0);
refinedEssentialMatrix = svd.getU().times(s).times(svd.getV().transpose());
refinedError = getError(refinedEssentialMatrix, imgpts1, imgpts2);
}
/**
* Return the estimated essential matrix.
*
* @return the essential matrix
*/
public Matrix getEssentialMatrix() {
return essentialMatrix;
}
/**
* <p>
* Return the refined essential matrix.
* </p>
* <p>
* The refined essential matrix's singular values are (s1, s2, 0).
* </p>
*
* @return the refined essential matrix.
*/
public Matrix getRefinedEssentialMatrix() {
return refinedEssentialMatrix;
}
/**
* Return the error of the essential matrix: � (x2^T * E * x1)^2
*
* @return the error
*/
public double getError() {
return error;
}
/**
* Return the error of the refined essential matrix: � (x2^T * E' * x1)^2
*
* @return the error
*/
public double getRefinedError() {
return refinedError;
}
}