@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);
}
@Override
public void decode(double[] param, CalibratedPoseAndPoint outputModel) {
int paramIndex = 0;
// first decode the transformation
for (int i = 0; i < numViews; i++) {
// don't decode if it is already known
if (knownView[i]) continue;
Se3_F64 se = outputModel.getWorldToCamera(i);
rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]);
RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR());
Vector3D_F64 T = se.getT();
T.x = param[paramIndex++];
T.y = param[paramIndex++];
T.z = param[paramIndex++];
}
// now decode the points
for (int i = 0; i < numPoints; i++) {
Point3D_F64 p = outputModel.getPoint(i);
p.x = param[paramIndex++];
p.y = param[paramIndex++];
p.z = param[paramIndex++];
}
}