/** Test against perfectly matched and a little bit of noise */ @Test public void test2D() { for (double noise = 0; noise <= 0.01; noise += 0.01) { // can only correct small changes Se2_F64 tran = new Se2_F64(0.01, -0.02, 0.01); List<Point2D_F64> srcPts = UtilPoint2D_F64.random(-10, 10, 30, rand); List<Point2D_F64> srcOrig = UtilPoint2D_F64.copy(srcPts); List<Point2D_F64> modelPts = new ArrayList<Point2D_F64>(); for (Point2D_F64 p : srcPts) { modelPts.add(SePointOps_F64.transform(tran, p, null)); } // add noise UtilPoint2D_F64.noiseNormal(modelPts, noise, rand); PointModel<Point2D_F64> model = new PointModel<Point2D_F64>(modelPts); StoppingCondition stop = new StoppingCondition(10, 0.1 * noise / srcPts.size() + 1e-8); IterativeClosestPoint<Se2_F64, Point2D_F64> alg = new IterativeClosestPoint<Se2_F64, Point2D_F64>(stop, new MotionSe2PointSVD_F64()); alg.setModel(model); alg.process(srcPts); Se2_F64 foundTran = alg.getPointsToModel(); checkTransform(srcOrig, modelPts, foundTran, noise * 10 + 1e-8); } }