Example PnP

Example of how to solve problems using the Perspective-N-Point (PnP) family of solvers in BoofCV. For the PnP problem you have N points with known 3D locations and 2D image coordinates and the goal is to solve camera's 6DOF pose. The minimum number of points to solve this problem is 3, with at least one additional point to resolve ambiguities between the solutions. With 4 or more points a unique solution can be found. PnP solvers are commonly used in visual odometry and other SFM problems.

Example Code:

• ExamplePnP.java

Concepts:

• 3D Perspective
• Perspective-N-Point (PnP)

Related Examples:

• Visual Odometry Stereo

Example Code

/**
 * The Perspective-n-Point problem or PnP for short, is a problem where there are N observations of points with
 * known 3D coordinates. The output is the pose of the camera observing the points. The minimal solution requires
 * 3-points but produces multiple solutions. BoofCV provide several solutions to the PnP problem and the example
 * below demonstrates how to use them.
 *
 * @author Peter Abeles
 */
public class ExamplePnP {

	// describes the intrinsic camera parameters.
	CameraPinholeBrown intrinsic = new CameraPinholeBrown(500,490,0,320,240,640,480).fsetRadial(0.1,-0.05);

	// Used to generate random observations
	Random rand = new Random(234);

	public static void main(String[] args) {
		// create an arbitrary transform from world to camera reference frames
		Se3_F64 worldToCamera = new Se3_F64();
		worldToCamera.getT().setTo(5, 10, -7);
		ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ,0.1,-0.3,0,worldToCamera.getR());

		ExamplePnP app = new ExamplePnP();

		// Let's generate observations with no outliers
		// NOTE: Image observations are in normalized image coordinates NOT pixels
		List<Point2D3D> observations = app.createObservations(worldToCamera,100);

		System.out.println("Truth:");
		worldToCamera.print();
		System.out.println();
		System.out.println("Estimated, assumed no outliers:");
		app.estimateNoOutliers(observations).print();
		System.out.println("Estimated, assumed that there are outliers:");
		app.estimateOutliers(observations).print();

		System.out.println();
		System.out.println("Adding outliers");
		System.out.println();

		// add a bunch of outliers
		app.addOutliers(observations, 50);
		System.out.println("Estimated, assumed no outliers:");
		app.estimateNoOutliers(observations).print();
		System.out.println("Estimated, assumed that there are outliers:");
		app.estimateOutliers(observations).print();
	}

	/**
	 * Assumes all observations actually match the correct/real 3D point
	 */
	public Se3_F64 estimateNoOutliers( List<Point2D3D> observations ) {

		// Compute a single solution using EPNP
		// 10 iterations is what JavaDoc recommends, but might need to be tuned.
		// 0 test points. This parameters is actually ignored because EPNP only returns a single solution
		Estimate1ofPnP pnp = FactoryMultiView.pnp_1(EnumPNP.EPNP, 10, 0);

		Se3_F64 worldToCamera = new Se3_F64();
		pnp.process(observations,worldToCamera);

		// For some applications the EPNP solution might be good enough, but let's refine it
		RefinePnP refine = FactoryMultiView.pnpRefine(1e-8,200);

		Se3_F64 refinedWorldToCamera = new Se3_F64();

		if( !refine.fitModel(observations,worldToCamera,refinedWorldToCamera) )
			throw new RuntimeException("Refined failed! Input probably bad...");

		return refinedWorldToCamera;
	}

	/**
	 * Uses robust techniques to remove outliers
	 */
	public Se3_F64 estimateOutliers( List<Point2D3D> observations ) {
		// We can no longer trust that each point is a real observation. Let's use RANSAC to separate the points
		// You will need to tune the number of iterations and inlier threshold!!!
		ModelMatcherMultiview<Se3_F64,Point2D3D> ransac =
				FactoryMultiViewRobust.pnpRansac(new ConfigPnP(),new ConfigRansac(300,1.0));
		ransac.setIntrinsic(0,intrinsic);

		// Observations must be in normalized image coordinates!  See javadoc of pnpRansac
		if( !ransac.process(observations) )
			throw new RuntimeException("Probably got bad input data with NaN inside of it");

		System.out.println("Inlier size "+ransac.getMatchSet().size());
		Se3_F64 worldToCamera = ransac.getModelParameters();

		// You will most likely want to refine this solution too. Can make a difference with real world data
		RefinePnP refine = FactoryMultiView.pnpRefine(1e-8,200);

		Se3_F64 refinedWorldToCamera = new Se3_F64();

		// notice that only the match set was passed in
		if( !refine.fitModel(ransac.getMatchSet(),worldToCamera,refinedWorldToCamera) )
			throw new RuntimeException("Refined failed! Input probably bad...");

		return refinedWorldToCamera;
	}

	/**
	 * Generates synthetic observations randomly in front of the camera. Observations are in normalized image
	 * coordinates and not pixels!  See {@link PerspectiveOps#convertPixelToNorm} for how to go from pixels
	 * to normalized image coordinates.
	 */
	public List<Point2D3D> createObservations( Se3_F64 worldToCamera , int total ) {

		Se3_F64 cameraToWorld = worldToCamera.invert(null);

		// transform from pixel coordinates to normalized pixel coordinates, which removes lens distortion
		Point2Transform2_F64 pixelToNorm = LensDistortionFactory.narrow(intrinsic).undistort_F64(true,false);

		List<Point2D3D> observations = new ArrayList<>();

		Point2D_F64 norm = new Point2D_F64();
		for (int i = 0; i < total; i++) {
			// randomly pixel a point inside the image
			double x = rand.nextDouble()*intrinsic.width;
			double y = rand.nextDouble()*intrinsic.height;

			// Convert to normalized image coordinates because that's what PNP needs.
			// it can't process pixel coordinates
			pixelToNorm.compute(x,y,norm);

			// Randomly pick a depth and compute 3D coordinate
			double Z = rand.nextDouble()+4;
			double X = norm.x*Z;
			double Y = norm.y*Z;

			// Change the point's reference frame from camera to world
			Point3D_F64 cameraPt = new Point3D_F64(X,Y,Z);
			Point3D_F64 worldPt = new Point3D_F64();

			SePointOps_F64.transform(cameraToWorld,cameraPt,worldPt);

			// Save the perfect noise free observation
			Point2D3D o = new Point2D3D();
			o.getLocation().setTo(worldPt);
			o.getObservation().setTo(norm.x,norm.y);

			observations.add(o);
		}

		return observations;
	}

	/**
	 * Adds some really bad observations to the mix
	 */
	public void addOutliers( List<Point2D3D> observations , int total ) {

		int size = observations.size();

		for (int i = 0; i < total; i++) {
			// outliers will be created by adding lots of noise to real observations
			Point2D3D p = observations.get(rand.nextInt(size));

			Point2D3D o = new Point2D3D();
			o.observation.setTo(p.observation);
			o.location.x = p.location.x + rand.nextGaussian()*5;
			o.location.y = p.location.y + rand.nextGaussian()*5;
			o.location.z = p.location.z + rand.nextGaussian()*5;

			observations.add(o);
		}

		// randomize the order
		Collections.shuffle(observations,rand);
	}
}