- unit tests and bug fixed

applications/src/boofcv/app/TrackFiducialWebcam.java

@@ -101,7 +101,7 @@ public static void main(String[] args) {
 
 		String patternPath = UtilIO.getPathToBase()+"data/applet/fiducial/image/";
 		SquareImage_to_FiducialDetector<ImageFloat32> detector =
-				FactoryFiducial.squareImageRobust(new ConfigFiducialImage(),6, ImageFloat32.class);
+				FactoryFiducial.squareImageFast(new ConfigFiducialImage(),100, ImageFloat32.class);
 		detector.addTarget(loadImage(patternPath+"ke.png", ImageFloat32.class),100,2.5);
 		detector.addTarget(loadImage(patternPath+"dog.png", ImageFloat32.class),100,2.5);
 		detector.addTarget(loadImage(patternPath+"yu.png", ImageFloat32.class),100,2.5);

change.txt

@@ -87,6 +87,7 @@ TODO
      * Regression:
         - Binary Motion Blur
         - Binary Static
+        - Both: Looking at it directly static stability.
      * Detector
         - Polygon reject noisy edges
         - Use border to reduce false positives

main/geo/src/boofcv/struct/calib/IntrinsicParameters.java

@@ -135,7 +135,7 @@ public boolean isDistorted() {
 		if( radial != null && radial.length > 0 ) {
 			for (int i = 0; i < radial.length; i++) {
 				if( radial[i] != 0 )
-					return false;
+					return true;
 			}
 		}
 		return t1 != 0 || t2 != 0;

main/geo/test/boofcv/struct/calib/TestIntrinsicParameters.java

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2011-2015, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of BoofCV (http://boofcv.org).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package boofcv.struct.calib;
+
+import org.junit.Test;
+
+import static org.junit.Assert.*;
+
+/**
+ * @author Peter Abeles
+ */
+public class TestIntrinsicParameters {
+
+	@Test
+	public void constructor_K() {
+		IntrinsicParameters p = new IntrinsicParameters(200,210,1,320,240,640,380);
+
+		assertEquals(200,p.fx,1e-8);
+		assertEquals(210,p.fy,1e-8);
+		assertEquals(1,p.skew,1e-8);
+		assertEquals(320,p.cx,1e-8);
+		assertEquals(240,p.cy,1e-8);
+		assertEquals(640,p.width,1e-8);
+		assertEquals(380,p.height,1e-8);
+		assertTrue(p.radial==null);
+		assertEquals(0,p.t1,1e-8);
+		assertEquals(0,p.t2, 1e-8);
+	}
+
+	@Test
+	public void fsetK() {
+		IntrinsicParameters p = new IntrinsicParameters(200,210,1,320,240,640,380);
+
+		assertTrue(p == p.fsetK(201, 211, 2, 321, 241, 641, 381));
+
+		assertEquals(201,p.fx,1e-8);
+		assertEquals(211,p.fy,1e-8);
+		assertEquals(2,p.skew,1e-8);
+		assertEquals(321,p.cx,1e-8);
+		assertEquals(241,p.cy,1e-8);
+		assertEquals(641,p.width,1e-8);
+		assertEquals(381,p.height,1e-8);
+		assertTrue(p.radial==null);
+		assertEquals(0,p.t1,1e-8);
+		assertEquals(0,p.t2, 1e-8);
+	}
+
+	@Test
+	public void fsetRadial() {
+		IntrinsicParameters p = new IntrinsicParameters(200,210,1,320,240,640,380);
+
+		assertTrue(p == p.fsetRadial(1.1,2.2,3.3));
+
+		assertTrue(p.radial.length==3);
+		assertEquals(1.1,p.radial[0],1e-8);
+		assertEquals(2.2,p.radial[1],1e-8);
+		assertEquals(3.3,p.radial[2],1e-8);
+
+		assertEquals(200,p.fx,1e-8);
+		assertEquals(210,p.fy,1e-8);
+		assertEquals(1,p.skew,1e-8);
+		assertEquals(320,p.cx,1e-8);
+		assertEquals(240,p.cy,1e-8);
+		assertEquals(640,p.width,1e-8);
+		assertEquals(380,p.height,1e-8);
+		assertEquals(0,p.t1,1e-8);
+		assertEquals(0,p.t2, 1e-8);
+	}
+
+	@Test
+	public void fsetTangental() {
+		IntrinsicParameters p = new IntrinsicParameters(200,210,1,320,240,640,380);
+
+		assertTrue(p == p.fsetTangental(1.1, 2.2));
+
+		assertEquals(1.1,p.t1,1e-8);
+		assertEquals(2.2,p.t2, 1e-8);
+
+		assertEquals(200,p.fx,1e-8);
+		assertEquals(210,p.fy,1e-8);
+		assertEquals(1,p.skew,1e-8);
+		assertEquals(320,p.cx,1e-8);
+		assertEquals(240,p.cy,1e-8);
+		assertEquals(640,p.width,1e-8);
+		assertEquals(380,p.height,1e-8);
+		assertTrue(p.radial==null);
+	}
+
+	@Test
+	public void isDistorted() {
+		IntrinsicParameters p = new IntrinsicParameters(200,210,0,320,240,640,380);
+
+		assertFalse(p.isDistorted());
+		assertFalse(p.fsetRadial(0,0).isDistorted());
+		assertTrue(p.fsetRadial(1,0).isDistorted());
+		assertFalse(p.fsetRadial(0,0).isDistorted());
+		assertTrue(p.fsetTangental(0, 0.1).isDistorted());
+	}
+}

main/recognition/src/boofcv/alg/fiducial/QuadPoseEstimator.java

@@ -45,33 +45,32 @@
 public class QuadPoseEstimator {
 
 	// provides set of hypotheses from 3 points
-	EstimateNofPnP p3p;
+	private EstimateNofPnP p3p;
 	// iterative refinement
-	RefinePnP refine;
+	private RefinePnP refine;
 
-	Estimate1ofPnP epnp = FactoryMultiView.computePnP_1(EnumPNP.EPNP,50,0);
+	private Estimate1ofPnP epnp = FactoryMultiView.computePnP_1(EnumPNP.EPNP,50,0);
 
 	// transforms from distorted pixel observation normalized image coordinates
-	PointTransform_F64 pixelToNorm;
-	PointTransform_F64 normToPixel;
+	private PointTransform_F64 pixelToNorm;
+	private PointTransform_F64 normToPixel;
 
 	// storage for inputs to estimation algorithms
-	Point2D3D[] points = new Point2D3D[4];
+	protected Point2D3D[] points = new Point2D3D[4];
 
-	List<Point2D_F64> listObs = new ArrayList<Point2D_F64>();
+	protected List<Point2D_F64> listObs = new ArrayList<Point2D_F64>();
 
-	List<Point2D3D> inputP3P = new ArrayList<Point2D3D>();
-	FastQueue<Se3_F64> solutions = new FastQueue(Se3_F64.class,true);
-	Se3_F64 refinedFiducialToCamera = new Se3_F64();
-	Se3_F64 foundEPNP = new Se3_F64();
+	private List<Point2D3D> inputP3P = new ArrayList<Point2D3D>();
+	private FastQueue<Se3_F64> solutions = new FastQueue(Se3_F64.class,true);
+	private Se3_F64 refinedFiducialToCamera = new Se3_F64();
+	private Se3_F64 foundEPNP = new Se3_F64();
 
-	Point3D_F64 cameraP3 = new Point3D_F64();
-	Point2D_F64 predicted = new Point2D_F64();
+	private Point3D_F64 cameraP3 = new Point3D_F64();
+	private Point2D_F64 predicted = new Point2D_F64();
 
 	// storage for when it searches for the best solution
-	double bestError;
-	Se3_F64 bestPose = new Se3_F64();
-	int bestIndex;
+	protected double bestError;
+	protected Se3_F64 bestPose = new Se3_F64();
 
 	/**
 	 * Constructor which picks reasonable and generally good algorithms for pose estimation.
@@ -141,16 +140,14 @@ public boolean process( Quadrilateral_F64 corners ) {
 
 		// estimate pose using all permutations
 		bestError = Double.MAX_VALUE;
-		estimate(0);
-		estimate(1);
-		estimate(2);
-		estimate(3);
+		estimateP3P(0);
+		estimateP3P(1);
+		estimateP3P(2);
+		estimateP3P(3);
 
 		if( bestError == Double.MAX_VALUE )
 			return false;
 
-//		System.out.println("bestError = "+bestError);
-
 		// refine the best estimate
 		inputP3P.clear();
 		for( int i = 0; i < 4; i++ ) {
@@ -165,30 +162,26 @@ public boolean process( Quadrilateral_F64 corners ) {
 					double error = computeErrors(foundEPNP);
 					if (error < bestError) {
 						bestPose.set(foundEPNP);
-//						System.out.println("    better epnp error = " + error);
 					}
 				}
 			}
 		}
 
-//		refinedFiducialToCamera.set(bestPose);
 		if( !refine.fitModel(inputP3P,bestPose,refinedFiducialToCamera) ) {
 			// us the previous estimate instead
 			refinedFiducialToCamera.set(bestPose);
 			return true;
 		}
 
-//		double refineError = computeErrors(refinedFiducialToCamera);
-//		System.out.println("    refined error = "+refineError);
-
 		return true;
 	}
 
 	/**
-	 * Estimates the pose from all put the excluded point
+	 * Estimates the pose using P3P from 3 out of 4 points.  Then use all 4 to pick the best solution
+	 *
 	 * @param excluded which corner to exclude and use to check the answers from the others
 	 */
-	private void estimate( int excluded ) {
+	protected void estimateP3P(int excluded) {
 
 		// the point used to check the solutions is the last one
 		inputP3P.clear();
@@ -213,18 +206,17 @@ private void estimate( int excluded ) {
 			if( error < bestError ) {
 				bestError = error;
 				bestPose.set(solutions.get(i));
-				bestIndex = excluded;
 			}
 		}
 
 	}
 
 	/**
-	 * Compute the sum of preprojection errors for all four points
+	 * Compute the sum of reprojection errors for all four points
 	 * @param fiducialToCamera Transform being evaluated
-	 * @return sum of error
+	 * @return sum of Euclidean-squared errors
 	 */
-	private double computeErrors(Se3_F64 fiducialToCamera ) {
+	protected double computeErrors(Se3_F64 fiducialToCamera ) {
 		if( fiducialToCamera.T.z < 0 ) {
 			// the low level algorithm should already filter this code, but just incase
 			return Double.MAX_VALUE;

main/recognition/test/boofcv/alg/fiducial/TestBaseDetectFiducialSquare.java

@@ -209,7 +209,8 @@ private static Point3D_F64 c( Point2D_F64 a ) {
 	}
 
 	/**
-	 * Ensure that lens distortion is being removed from the fiducial square.
+	 * Ensure that lens distortion is being removed from the fiducial square.  All check to make sure the class
+	 * updates everything when init is called again with a different model.
 	 */
 	@Test
 	public void lensRemoval() {
@@ -219,8 +220,18 @@ public void lensRemoval() {
 		expected.add( new Point2D_F64(10+120,350));
 		expected.add( new Point2D_F64(10+120,350+120));
 
-		IntrinsicParameters intrinsic = new IntrinsicParameters(500,500,0,320,240,640,480).fsetRadial(-0.15,-0.05);
+		DetectCorner detector = new DetectCorner();
+
+		IntrinsicParameters intrinsic = new IntrinsicParameters(500,500,0,320,240,640,480).fsetRadial(-0.1,-0.05);
+
+		detectWithLensDistortion(expected, detector, intrinsic);
+
+		intrinsic = new IntrinsicParameters(500,500,0,320,240,640,480).fsetRadial(0.1,0.05);
+
+		detectWithLensDistortion(expected, detector, intrinsic);
+	}
 
+	private void detectWithLensDistortion(List<Point2D_F64> expected, DetectCorner detector, IntrinsicParameters intrinsic) {
 		// create a pattern with a corner for orientation and put it into the image
 		ImageUInt8 pattern = createPattern(6*20, true);
 		ImageUInt8 image = new ImageUInt8(640,480);
@@ -238,8 +249,6 @@ public void lensRemoval() {
 		ImageUInt8 distorted = new ImageUInt8(640,480);
 		distorter.apply(image,distorted);
 
-		DetectCorner detector = new DetectCorner();
-
 		detector.configure(intrinsic, false);
 		detector.process(distorted);
 
@@ -256,16 +265,9 @@ public void lensRemoval() {
 			assertTrue(f.distance(e) <= 0.4 );
 		}
 
-		// the check to see if square is correctly undistorted is inside the processing function itself
+		// The check to see if square is correctly undistorted is inside the processing function itself
 	}
 
-	/**
-	 * Keep image size the same but change lens distortion parameters.  See everything still works.
-	 */
-	@Test
-	public void changeLensDistortion() {
-		fail("implement");
-	}
 
 	/**
 	 * Creates a square pattern image of the specified size