- improved stability of fiducials at a distance viewed head on

- improved visualization

change.txt

@@ -29,6 +29,7 @@ Version : Alpha 0.19
   * Square based fiducials now use the new polygon detector and benefit from its improved subpixel
   * Fixed bug where the returned orientation was inconsistent with the JavaDoc
   * Fixed another bug that caused the sides to "flip" depending on view angle
+  * Handling of small far away fiducials being viewed head on has improved much
 - PerspectiveOps
   * Can render pixel in camera frame using IntrinsicParameters
 - Created FactoryMultiViewRobust for simplifying the creation of robust versions of

integration/pyboof/java/src/pyboof/PyBoofEntryPoint.java

@@ -28,7 +28,6 @@
  */
 public class PyBoofEntryPoint {
 
-
 	public static void main(String[] args) {
 		GatewayServer gatewayServer = new GatewayServer(new PyBoofEntryPoint());
 		gatewayServer.start();

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

@@ -27,6 +27,7 @@
 import boofcv.struct.calib.IntrinsicParameters;
 import boofcv.struct.distort.PointTransform_F64;
 import boofcv.struct.geo.Point2D3D;
+import georegression.geometry.UtilPolygons2D_F64;
 import georegression.struct.point.Point2D_F64;
 import georegression.struct.point.Point3D_F64;
 import georegression.struct.se.Se3_F64;
@@ -44,6 +45,16 @@
  */
 public class QuadPoseEstimator {
 
+	// if the target is less than or equals to this number of pixels along a side then it is considered small
+	// and a special case will be handled.
+	// I think this threshold should be valid across different resolution images.  Corner accuracy should be
+	// less than a pixel and it becomes unstable because changes in hangle result in an error of less than a pixel
+	// when the target is small
+	public static final double SMALL_PIXELS = 60.0;
+
+	// the adjusted solution is accepted if it doesn't increase the pixel reprojection error more than this amount
+	public static final double FUDGE_FACTOR = 0.2;
+
 	// provides set of hypotheses from 3 points
 	private EstimateNofPnP p3p;
 	// iterative refinement
@@ -52,6 +63,7 @@ public class QuadPoseEstimator {
 	private Estimate1ofPnP epnp = FactoryMultiView.computePnP_1(EnumPNP.EPNP,50,0);
 
 	// transforms from distorted pixel observation normalized image coordinates
+	private PointTransform_F64 distortedToUndistorted;
 	private PointTransform_F64 pixelToNorm;
 	private PointTransform_F64 normToPixel;
 
@@ -62,7 +74,7 @@ public class QuadPoseEstimator {
 
 	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 outputFiducialToCamera = new Se3_F64();
 	private Se3_F64 foundEPNP = new Se3_F64();
 
 	private Point3D_F64 cameraP3 = new Point3D_F64();
@@ -72,6 +84,14 @@ public class QuadPoseEstimator {
 	protected double bestError;
 	protected Se3_F64 bestPose = new Se3_F64();
 
+	// predeclared internal work space.  Minimizing new memory
+	IntrinsicParameters intrinsicUndist = new IntrinsicParameters();
+	Quadrilateral_F64 undistortedCorners = new Quadrilateral_F64();
+	Quadrilateral_F64 enlargedCorners = new Quadrilateral_F64();
+	Se3_F64 foundEnlarged = new Se3_F64();
+	Se3_F64 foundRegular = new Se3_F64();
+	Point2D_F64 center = new Point2D_F64();
+
 	/**
 	 * Constructor which picks reasonable and generally good algorithms for pose estimation.
 	 *
@@ -100,8 +120,13 @@ public QuadPoseEstimator(EstimateNofPnP p3p, RefinePnP refine) {
 	 * @param intrinsic Intrinsic camera parameters
 	 */
 	public void setIntrinsic( IntrinsicParameters intrinsic ) {
-		pixelToNorm = LensDistortionOps.transformPoint(intrinsic).undistort_F64(true,false);
-		normToPixel = LensDistortionOps.transformPoint(intrinsic).distort_F64(false, true);
+		distortedToUndistorted =  LensDistortionOps.transformPoint(intrinsic).undistort_F64(true,true);
+
+		intrinsicUndist.fsetK(intrinsic.fx,intrinsic.fy,intrinsic.skew,intrinsic.cx,intrinsic.cy,
+				intrinsic.width,intrinsic.height);
+
+		pixelToNorm = LensDistortionOps.transformPoint(intrinsicUndist).undistort_F64(true,false);
+		normToPixel = LensDistortionOps.transformPoint(intrinsicUndist).distort_F64(false, true);
 	}
 
 	/**
@@ -125,14 +150,78 @@ public void setFiducial( double x0 , double y0 , double x1 , double y1 ,
 	 */
 	public boolean process( Quadrilateral_F64 corners ) {
 
+		// put quad into undistorted pixels so that weird stuff doesn't happen when it expands
+
+		distortedToUndistorted.compute(corners.a.x, corners.a.y, undistortedCorners.a);
+		distortedToUndistorted.compute(corners.b.x, corners.b.y, undistortedCorners.b);
+		distortedToUndistorted.compute(corners.c.x, corners.c.y, undistortedCorners.c);
+		distortedToUndistorted.compute(corners.d.x, corners.d.y, undistortedCorners.d);
+
+		double length0 =  undistortedCorners.getSideLength(0);
+		double length1 =  undistortedCorners.getSideLength(1);
+
+		double ratio = Math.max(length0,length1)/Math.min(length0,length1);
+
+		// this is mainly an optimization thing.  The handling of the pathological cause is only
+		// used if it doesn't add a bunch of error.  But this technique is only needed when certain conditions
+		// are meet.
+		if( ratio < 1.3 && length0 < SMALL_PIXELS && length1 < SMALL_PIXELS ) {
+			return estimatePathological();
+		} else {
+			return estimate(undistortedCorners, outputFiducialToCamera);
+		}
+	}
+
+	/**
+	 * Estimating the orientation is difficult when looking directly at a fiducial head on.  Basically
+	 * when the target appears close to a perfect square.  What I believe is happening is that when
+	 * the target is small significant changes in orientation only cause a small change in reprojection
+	 * error.  So it over fits location and comes up with some crazy orientation.
+	 *
+	 * To add more emphasis on orientation  target is enlarged, which shouldn't change orientation, but now a small
+	 * change in orientation results in a large error.  The translational component is still estimated the
+	 * usual way.  To ensure that this technique isn't hurting the state estimate too much it checks to
+	 * see if the error has increased too much.
+	 *
+	 * @return true if successful false if not
+	 */
+	private boolean estimatePathological() {
+		enlargedCorners.set(undistortedCorners);
+		enlarge(enlargedCorners, 4);
+
+		if( !estimate(enlargedCorners, foundEnlarged) )
+			return false;
+
+		if( !estimate(undistortedCorners, foundRegular) )
+			return false;
+
+		double errorRegular = computeErrors(foundRegular);
+
+		outputFiducialToCamera.getT().set(foundRegular.getT());
+		outputFiducialToCamera.getR().set(foundEnlarged.getR());
+
+		double errorModified = computeErrors(outputFiducialToCamera);
+
+		// if the solutions are very similar go with the enlarged version
+		if (errorModified > errorRegular + FUDGE_FACTOR ) {
+			outputFiducialToCamera.set(foundRegular);
+		}
+		return true;
+	}
+
+	/**
+	 * Given the observed corners of the quad in the image in pixels estimate and store the results
+	 * of its pose
+	 */
+	protected boolean estimate( Quadrilateral_F64 corners , Se3_F64 foundFiducialToCamera ) {
 		// put it into a list to simplify algorithms
 		listObs.clear();
 		listObs.add( corners.a );
 		listObs.add( corners.b );
 		listObs.add( corners.c );
 		listObs.add( corners.d );
 
-		// convert obervations into normalized image coordinates which P3P requires
+		// convert observations into normalized image coordinates which P3P requires
 		pixelToNorm.compute(corners.a.x,corners.a.y,points[0].observation);
 		pixelToNorm.compute(corners.b.x,corners.b.y,points[1].observation);
 		pixelToNorm.compute(corners.c.x,corners.c.y,points[2].observation);
@@ -160,16 +249,17 @@ public boolean process( Quadrilateral_F64 corners ) {
 			if (epnp.process(inputP3P, foundEPNP)) {
 				if( foundEPNP.T.z > 0 ) {
 					double error = computeErrors(foundEPNP);
+//					System.out.println("   error EPNP = "+error);
 					if (error < bestError) {
 						bestPose.set(foundEPNP);
 					}
 				}
 			}
 		}
 
-		if( !refine.fitModel(inputP3P,bestPose,refinedFiducialToCamera) ) {
+		if( !refine.fitModel(inputP3P,bestPose,foundFiducialToCamera) ) {
 			// us the previous estimate instead
-			refinedFiducialToCamera.set(bestPose);
+			foundFiducialToCamera.set(bestPose);
 			return true;
 		}
 
@@ -211,6 +301,24 @@ protected void estimateP3P(int excluded) {
 
 	}
 
+	/**
+	 * Enlarges the quadrilateral to make it more sensitive to changes in orientation
+	 */
+	protected void enlarge( Quadrilateral_F64 corners, double scale ) {
+
+		UtilPolygons2D_F64.center(corners, center);
+
+		extend(center,corners.a,scale);
+		extend(center,corners.b,scale);
+		extend(center,corners.c,scale);
+		extend(center,corners.d,scale);
+	}
+
+	protected void extend( Point2D_F64 pivot , Point2D_F64 corner , double scale ) {
+		corner.x = pivot.x + (corner.x-pivot.x)*scale;
+		corner.y = pivot.y + (corner.y-pivot.y)*scale;
+	}
+
 	/**
 	 * Compute the sum of reprojection errors for all four points
 	 * @param fiducialToCamera Transform being evaluated
@@ -222,24 +330,24 @@ protected double computeErrors(Se3_F64 fiducialToCamera ) {
 			return Double.MAX_VALUE;
 		}
 
-		double error = 0;
+		double maxError = 0;
 
 		for( int i = 0; i < 4; i++ ) {
-			error += computePixelError(fiducialToCamera,points[i].location,listObs.get(i));
+			maxError = Math.max(maxError,computePixelError(fiducialToCamera, points[i].location, listObs.get(i)));
 		}
 
-		return error;
+		return maxError;
 	}
 
 	private double computePixelError( Se3_F64 fiducialToCamera , Point3D_F64 X , Point2D_F64 pixel ) {
 		SePointOps_F64.transform(fiducialToCamera,X,cameraP3);
 
 		normToPixel.compute( cameraP3.x/cameraP3.z , cameraP3.y/cameraP3.z , predicted );
 
-		return predicted.distance2(pixel);
+		return predicted.distance(pixel);
 	}
 
 	public Se3_F64 getWorldToCamera() {
-		return refinedFiducialToCamera;
+		return outputFiducialToCamera;
 	}
 }

main/recognition/src/boofcv/factory/fiducial/ConfigFiducialBinary.java

@@ -38,7 +38,7 @@ public class ConfigFiducialBinary implements Configuration {
 	 * Value from 0 to 1.  0 is very strict and 1 is very relaxed.  Used when classifying a require block
 	 * as black or white.  If it can't be classified then the shape is discarded
 	 */
-	public double ambiguousThreshold = 0.4;
+	public double ambiguousThreshold = 0.5;
 
 	/**
 	 * Configuration for square detector

main/visualize/src/boofcv/gui/feature/VisualizeShapes.java

@@ -31,6 +31,8 @@
 import java.awt.geom.Line2D;
 import java.util.List;
 
+import static boofcv.gui.fiducial.VisualizeFiducial.drawLine;
+
 /**
  * @author Peter Abeles
  */
@@ -113,6 +115,29 @@ public static void drawPolygon( Polygon2D_F64 polygon, boolean loop, Graphics2D
 		}
 	}
 
+	public static void drawPolygon( Polygon2D_F64 polygon, boolean loop, Graphics2D g2 , boolean interpolate ) {
+		if( interpolate ) {
+			g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
+			g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
+
+			Line2D.Double l = new Line2D.Double();
+
+			for( int i = 0; i < polygon.size()-1; i++ ) {
+				Point2D_F64 p0 = polygon.get(i);
+				Point2D_F64 p1 = polygon.get(i+1);
+				drawLine(g2, l, p0.x,  p0.y,  p1.x,  p1.y);
+			}
+			if( loop && polygon.size() > 0) {
+				Point2D_F64 p0 = polygon.get(0);
+				Point2D_F64 p1 = polygon.get(polygon.size()-1);
+				drawLine(g2, l, p0.x, p0.y, p1.x, p1.y);
+			}
+
+		} else {
+			drawPolygon(polygon,loop,g2);
+		}
+	}
+
 	/**
 	 * Draws the rotated ellipse
 	 * @param ellipse Description of the ellipse

main/visualize/src/boofcv/gui/fiducial/VisualizeFiducial.java

@@ -22,7 +22,6 @@
 import boofcv.alg.geo.WorldToCameraToPixel;
 import boofcv.struct.calib.IntrinsicParameters;
 import georegression.struct.point.Point2D_F64;
-import georegression.struct.point.Point2D_I32;
 import georegression.struct.point.Point3D_F64;
 import georegression.struct.se.Se3_F64;
 
@@ -82,18 +81,17 @@ public static void drawCube( Se3_F64 targetToCamera , IntrinsicParameters intrin
 		corners[6] = new Point3D_F64( r, r,h);
 		corners[7] = new Point3D_F64(-r, r,h);
 
-		Point2D_I32 pixel[] = new Point2D_I32[8];
+		Point2D_F64 pixel[] = new Point2D_F64[8];
 		Point2D_F64 p = new Point2D_F64();
 		WorldToCameraToPixel transform = PerspectiveOps.createWorldToPixel(intrinsic,targetToCamera);
 		for (int i = 0; i < 8; i++) {
-			Point3D_F64 c = corners[i];
-			if( !transform.transform(c,p) )
+			if( !transform.transform(corners[i],p) )
 				throw new RuntimeException("Crap");
-			pixel[i] = new Point2D_I32((int)(p.x+0.5),(int)(p.y+0.5));
+			pixel[i] = p.copy();
 		}
 
 		Line2D.Double l = new Line2D.Double();
-		g2.setStroke(new BasicStroke(4));
+		g2.setStroke(new BasicStroke(2));
 		g2.setColor(Color.RED);
 
 		drawLine(g2,l,pixel[0].x, pixel[0].y, pixel[1].x, pixel[1].y);
@@ -117,7 +115,7 @@ public static void drawCube( Se3_F64 targetToCamera , IntrinsicParameters intrin
 		drawLine(g2,l,pixel[7].x,pixel[7].y,pixel[4].x,pixel[4].y);
 	}
 
-	private static void drawLine( Graphics2D g2 , Line2D.Double line , double x0 , double y0 , double x1 , double y1 ) {
+	public static void drawLine( Graphics2D g2 , Line2D.Double line , double x0 , double y0 , double x1 , double y1 ) {
 		line.setLine(x0,y0,x1,y1);
 		g2.draw(line);
 	}