@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++];
}
}
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);
}
}
@Override
public double computeDistance(PointVectorNN pv) {
n.set(pv.p.x - model.center.x, pv.p.y - model.center.y, pv.p.z - model.center.z);
n.normalize();
if (Math.abs(n.dot(pv.normal)) < tolAngleCosine) return Double.MAX_VALUE;
return Math.abs(Distance3D_F64.distance(model, pv.p));
}
@Test
public void multTran_2d_3d() {
Vector2D_F64 a = new Vector2D_F64(-1, 2);
DenseMatrix64F M = new DenseMatrix64F(3, 3, true, 1, 2, 3, 4, 5, 6, 7, 8, 9);
Vector3D_F64 c = new Vector3D_F64();
GeometryMath_F64.multTran(M, a, c);
assertEquals(14, c.getX(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(16, c.getY(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(18, c.getZ(), GrlConstants.DOUBLE_TEST_TOL);
}
@Test
public void sub() {
Vector3D_F64 a = new Vector3D_F64(1, 2, 3);
Vector3D_F64 b = new Vector3D_F64(3, 1, 4);
Vector3D_F64 c = new Vector3D_F64();
GeometryMath_F64.sub(a, b, c);
assertEquals(-2, c.getX(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(1, c.getY(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(-1, c.getZ(), GrlConstants.DOUBLE_TEST_TOL);
}
@Test
public void add_scale() {
Vector3D_F64 a = new Vector3D_F64(1, 2, 3);
Vector3D_F64 b = new Vector3D_F64(3, 1, 4);
Vector3D_F64 c = new Vector3D_F64();
GeometryMath_F64.add(2, a, -1, b, c);
assertEquals(-1, c.getX(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(3, c.getY(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(2, c.getZ(), GrlConstants.DOUBLE_TEST_TOL);
}
@Test
public void addMult() {
Vector3D_F64 a = new Vector3D_F64(1, 2, 3);
Vector3D_F64 b = new Vector3D_F64(2, 3, 4);
Vector3D_F64 c = new Vector3D_F64();
DenseMatrix64F M = new DenseMatrix64F(3, 3, true, 1, 1, 1, 1, 1, 1, 1, 1, 1);
GeometryMath_F64.addMult(a, M, b, c);
assertEquals(10, c.getX(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(11, c.getY(), GrlConstants.DOUBLE_TEST_TOL);
assertEquals(12, c.getZ(), GrlConstants.DOUBLE_TEST_TOL);
}
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));
}
/**
* Randomly generate points on a plane by randomly selecting two vectors on the plane using cross
* products
*/
private List<Point3D_F64> randPointOnPlane(PlaneNormal3D_F64 plane, int N) {
Vector3D_F64 v = new Vector3D_F64(-2, 0, 1);
Vector3D_F64 a = UtilTrig_F64.cross(plane.n, v);
a.normalize();
Vector3D_F64 b = UtilTrig_F64.cross(plane.n, a);
b.normalize();
List<Point3D_F64> ret = new ArrayList<Point3D_F64>();
for (int i = 0; i < N; i++) {
double v0 = rand.nextGaussian();
double v1 = rand.nextGaussian();
Point3D_F64 p = new Point3D_F64();
p.x = plane.p.x + v0 * a.x + v1 * b.x;
p.y = plane.p.y + v0 * a.y + v1 * b.y;
p.z = plane.p.z + v0 * a.z + v1 * b.z;
ret.add(p);
}
return ret;
}