public void evaluateObservationNoise(
double minPixelError, double maxPixelError, int N, boolean isPlanar) {
System.out.println("------------------------");
rand = new Random(234);
for (int i = 0; i <= N; i++) {
double mag = (maxPixelError - minPixelError) * i / N + minPixelError;
init(NUM_POINTS, false, isPlanar);
addPixelNoise(mag);
if (!target.process(observationPose, found))
throw new RuntimeException("Not expected to fail");
double expectedEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(motion.getR());
double foundEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(found.getR());
Vector3D_F64 expectedTran = motion.getT();
Vector3D_F64 foundTran = found.getT();
double errorTran = expectedTran.distance(foundTran);
double errorEuler = 0;
double sum = 0;
for (int j = 0; j < 3; j++) {
double e = expectedEuler[j] - foundEuler[j];
errorEuler += e * e;
sum += expectedEuler[j];
}
errorEuler = 100 * Math.sqrt(errorEuler) / Math.sqrt(sum);
System.out.printf("%3d angle %6.2f%% translation %6.2e\n", i, errorEuler, errorTran);
}
}
public void evaluateMinimal(double pixelSigma, boolean isPlanar, int numTrials) {
// make sure each test case has the same random seed
rand = new Random(234);
double totalEuler = 0;
double totalTran = 0;
int totalFail = 0;
for (int i = 0; i < numTrials; i++) {
init(target.getMinimumPoints(), false, isPlanar);
addPixelNoise(pixelSigma);
if (!target.process(observationPose, found)) {
totalFail++;
continue;
}
double expectedEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(motion.getR());
double foundEuler[] = RotationMatrixGenerator.matrixToEulerXYZ(found.getR());
Vector3D_F64 expectedTran = motion.getT();
Vector3D_F64 foundTran = found.getT();
double errorTran = expectedTran.distance(foundTran);
double errorEuler = 0;
double sum = 0;
for (int j = 0; j < 3; j++) {
double e = expectedEuler[j] - foundEuler[j];
errorEuler += e * e;
sum += expectedEuler[j];
}
errorEuler = 100 * Math.sqrt(errorEuler) / Math.sqrt(sum);
// System.out.println(errorEuler);
totalEuler += errorEuler;
totalTran += errorTran;
}
int N = numTrials - totalFail;
System.out.printf(
"%20s N = %d failed %.3f%% euler = %3f%% tran = %5f\n",
name,
target.getMinimumPoints(),
(totalFail / (double) numTrials),
(totalEuler / N),
(totalTran / N));
}
private void perfectObservations(int numSample) {
init(numSample, false);
List<PointPosePair> inputs = new ArrayList<PointPosePair>();
for (int i = 0; i < currentObs.size(); i++) {
Point2D_F64 o = currentObs.get(i);
Point3D_F64 X = worldPts.get(i);
inputs.add(new PointPosePair(o, X));
}
Se3_F64 found = new Se3_F64();
assertTrue(alg.process(inputs, found));
assertTrue(MatrixFeatures.isIdentical(worldToCamera.getR(), found.getR(), 1e-8));
assertTrue(found.getT().isIdentical(worldToCamera.getT(), 1e-8));
}
@Override
public boolean generate(List<Point2D3D> dataSet, Se3_F64 model) {
return alg.process(dataSet, model);
}