@Test
public void incorrectInput() {
init(30, false);
// compute true essential matrix
DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());
// create an alternative incorrect matrix
Vector3D_F64 T = worldToCamera.getT().copy();
T.x += 0.1;
DenseMatrix64F Emod = MultiViewOps.createEssential(worldToCamera.getR(), T);
ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();
// compute and compare results
assertTrue(alg.fitModel(pairs, Emod, found));
// normalize to allow comparison
CommonOps.divide(E.get(2, 2), E);
CommonOps.divide(Emod.get(2, 2), Emod);
CommonOps.divide(found.get(2, 2), found);
double error0 = 0;
double error1 = 0;
// very crude error metric
for (int i = 0; i < 9; i++) {
error0 += Math.abs(Emod.data[i] - E.data[i]);
error1 += Math.abs(found.data[i] - E.data[i]);
}
// System.out.println("error "+error1+" other "+error0);
assertTrue(error1 < error0);
}
/** H0 = H*M P=[M|m] from canonical camera */
private SimpleMatrix computeHZero(DenseMatrix64F F, Point3D_F64 e2, SimpleMatrix H) {
Vector3D_F64 v = new Vector3D_F64(.1, 0.5, .2);
// need to make sure M is not singular for this technique to work
SimpleMatrix P = SimpleMatrix.wrap(MultiViewOps.canonicalCamera(F, e2, v, 1));
SimpleMatrix M = P.extractMatrix(0, 3, 0, 3);
return H.mult(M);
}
@Test
public void perfectInput() {
init(30, false);
// compute true essential matrix
DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());
ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();
// give it the perfect matrix and see if it screwed it up
assertTrue(alg.fitModel(pairs, E, found));
// normalize so that they are the same
CommonOps.divide(E.get(2, 2), E);
CommonOps.divide(found.get(2, 2), found);
assertTrue(MatrixFeatures.isEquals(E, found, 1e-8));
}
/**
* Compute rectification transforms for the stereo pair given a fundamental matrix and its
* observations.
*
* @param F Fundamental matrix
* @param observations Observations used to compute F
* @param width Width of first image.
* @param height Height of first image.
*/
public void process(DenseMatrix64F F, List<AssociatedPair> observations, int width, int height) {
int centerX = width / 2;
int centerY = height / 2;
MultiViewOps.extractEpipoles(F, epipole1, epipole2);
checkEpipoleInside(width, height);
// compute the transform H which will send epipole2 to infinity
SimpleMatrix R = rotateEpipole(epipole2, centerX, centerY);
SimpleMatrix T = translateToOrigin(centerX, centerY);
SimpleMatrix G = computeG(epipole2, centerX, centerY);
SimpleMatrix H = G.mult(R).mult(T);
// Find the two matching transforms
SimpleMatrix Hzero = computeHZero(F, epipole2, H);
SimpleMatrix Ha = computeAffineH(observations, H.getMatrix(), Hzero.getMatrix());
rect1.set(Ha.mult(Hzero).getMatrix());
rect2.set(H.getMatrix());
}