/** Give it a grid and see if it computed a legitimate homography */
@Test
public void basicTest() {
// create a grid an apply an arbitrary transform to it
PlanarCalibrationTarget config = GenericCalibrationGrid.createStandardConfig();
DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.02, -0.05, 0.01, null);
Vector3D_F64 T = new Vector3D_F64(0, 0, -1000);
Se3_F64 motion = new Se3_F64(R, T);
List<Point2D_F64> observations = GenericCalibrationGrid.observations(motion, config);
// compute the homography
Zhang99ComputeTargetHomography alg = new Zhang99ComputeTargetHomography(config);
assertTrue(alg.computeHomography(observations));
DenseMatrix64F H = alg.getHomography();
// test this homography property: x2 = H*x1
List<Point2D_F64> gridPoints = config.points;
for (int i = 0; i < observations.size(); i++) {
Point2D_F64 a = GeometryMath_F64.mult(H, gridPoints.get(i), new Point2D_F64());
double diff = a.distance(observations.get(i));
assertEquals(0, diff, 1e-8);
}
}
/** The two sets of points match perfectly and with a little bit of noise */
@Test
public void test3D() {
for (double noise = 0; noise <= 0.01; noise += 0.01) {
// can only correct small changes
DenseMatrix64F R = RotationMatrixGenerator.eulerXYZ(0.01, -0.002, 0.03, null);
Vector3D_F64 T = new Vector3D_F64(0.02, 0.03, 0.01);
Se3_F64 tran = new Se3_F64(R, T);
List<Point3D_F64> srcPts = UtilPoint3D_F64.random(-10, 10, 30, rand);
List<Point3D_F64> srcOrig = UtilPoint3D_F64.copy(srcPts);
List<Point3D_F64> modelPts = new ArrayList<Point3D_F64>();
for (Point3D_F64 p : srcPts) {
modelPts.add(SePointOps_F64.transform(tran, p, null));
}
// add noise
UtilPoint3D_F64.noiseNormal(modelPts, noise, rand);
PointModel<Point3D_F64> model = new PointModel<Point3D_F64>(modelPts);
StoppingCondition stop = new StoppingCondition(10, 0.1 * noise / srcPts.size() + 1e-8);
IterativeClosestPoint<Se3_F64, Point3D_F64> alg =
new IterativeClosestPoint<Se3_F64, Point3D_F64>(stop, new MotionSe3PointSVD_F64());
alg.setModel(model);
alg.process(srcPts);
Se3_F64 foundTran = alg.getPointsToModel();
checkTransform(srcOrig, modelPts, foundTran, noise * 10 + 1e-8);
}
}