boofcv.abst.fiducial.FiducialDetectorPnP Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-recognition Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.8
/*
 * Copyright (c) 2011-2018, 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.abst.fiducial;

import boofcv.abst.geo.Estimate1ofPnP;
import boofcv.abst.geo.RefinePnP;
import boofcv.alg.distort.LensDistortionNarrowFOV;
import boofcv.factory.geo.FactoryMultiView;
import boofcv.struct.distort.Point2Transform2_F64;
import boofcv.struct.geo.Point2D3D;
import boofcv.struct.geo.PointIndex2D_F64;
import boofcv.struct.image.ImageBase;
import georegression.geometry.ConvertRotation3D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.so.Rodrigues_F64;

import java.util.ArrayList;
import java.util.List;

/**
 * Provides everything you need to convert a image based fiducial detector into one which can estimate
 * the fiducial's pose given control points.  The camera pose is found using a solution to the Pose-N-Point (PnP)
 * problem.
 *
 * Stability is computed by perturbing each control point by the user provided amount of disturbance.  The largest
 * delta for location and orientation is then found and saved.
 *
 * @author Peter Abeles
 */
public abstract class FiducialDetectorPnP>
		implements FiducialDetector {

	private LensDistortionNarrowFOV lensDistortion;

	// transform to remove lens distortion
	private Point2Transform2_F64 pixelToNorm;

	// 2D-3D pairs for just the detected points
	private List detected2D3D = new ArrayList<>();
	// list of the pixel observations for the most recently requested fiducial
	private List detectedPixels;
	private Point2D_F64 workPt = new Point2D_F64();

	// if a lens distortion model was set or not
	boolean hasCameraModel = false;

	// non-linear refinement of pose estimate
	private Estimate1ofPnP estimatePnP = FactoryMultiView.computePnPwithEPnP(10, 0.2);
	private RefinePnP refinePnP = FactoryMultiView.refinePnP(1e-8,100);

	// when computing the pose, this is the initial estimate before non-linear refinement
	private Se3_F64 initialEstimate = new Se3_F64();

	// max found location and orientation error when computing stability
	private double maxLocation;
	private double maxOrientation;

	// Work space for computing stability
	private Se3_F64 targetToCamera = new Se3_F64();
	private Se3_F64 targetToCameraSample = new Se3_F64();
	private Se3_F64 referenceCameraToTarget = new Se3_F64();
	private Se3_F64 difference = new Se3_F64();
	private Rodrigues_F64 rodrigues = new Rodrigues_F64();

	/**
	 * Estimates the stability by perturbing each land mark by the specified number of pixels in the distorted image.
	 */
	@Override
	public boolean computeStability(int which, double disturbance, FiducialStability results) {
		if( !getFiducialToCamera(which, targetToCamera))
			return false;
		targetToCamera.invert(referenceCameraToTarget);

		maxOrientation = 0;
		maxLocation = 0;

		for (int i = 0; i < detected2D3D.size(); i++) {
			Point2D3D p23 = detected2D3D.get(i);
			Point2D_F64 p = detectedPixels.get(i);
			workPt.set(p);
			perturb(which, disturbance, workPt, p, p23);
		}

		results.location = maxLocation;
		results.orientation = maxOrientation;

		return true;
	}

	private void perturb( int which, double disturbance ,
						  Point2D_F64 pixel , Point2D_F64 original , Point2D3D p23 ) {
		pixel.x = original.x + disturbance;
		computeDisturbance(which, pixel, p23);
		pixel.x = original.x - disturbance;
		computeDisturbance(which, pixel, p23);
		pixel.y = original.y;
		pixel.y = original.y + disturbance;
		computeDisturbance(which, pixel, p23);
		pixel.y = original.y - disturbance;
		computeDisturbance(which, pixel, p23);
	}

	private void computeDisturbance(int which, Point2D_F64 pixel, Point2D3D p23) {
		pixelToNorm.compute(pixel.x,pixel.y,p23.observation);
		if( estimatePose(which, detected2D3D, targetToCameraSample) ) {
			referenceCameraToTarget.concat(targetToCameraSample, difference);

			double d = difference.getT().norm();
			ConvertRotation3D_F64.matrixToRodrigues(difference.getR(), rodrigues);
			double theta = Math.abs(rodrigues.theta);
			if (theta > maxOrientation) {
				maxOrientation = theta;
			}
			if (d > maxLocation) {
				maxLocation = d;
			}
		}
	}

	@Override
	public void setLensDistortion(LensDistortionNarrowFOV distortion, int width , int height ) {
		if( distortion != null ) {
			this.hasCameraModel = true;
			this.lensDistortion = distortion;
			this.pixelToNorm = lensDistortion.undistort_F64(true, false);
		} else {
			this.hasCameraModel = false;
			this.lensDistortion = null;
			this.pixelToNorm = null;
		}
	}

	@Override
	public LensDistortionNarrowFOV getLensDistortion() {
		return lensDistortion;
	}

	@Override
	public boolean getFiducialToCamera(int which, Se3_F64 fiducialToCamera) {
		if( !hasCameraModel )
			return false;

		detectedPixels = getDetectedControl(which);
		if( detectedPixels.size() < 3 )
			return false;

		// 2D-3D point associations
		createDetectedList(which, detectedPixels);

		return estimatePose(which, detected2D3D, fiducialToCamera);
	}

	/**
	 * Create the list of observed points in 2D3D
	 */
	private void createDetectedList(int which, List pixels) {
		detected2D3D.clear();
		List all = getControl3D(which);
		for (int i = 0; i < pixels.size(); i++) {
			PointIndex2D_F64 a = pixels.get(i);
			Point2D3D b = all.get(i);

			pixelToNorm.compute(a.x,a.y, b.observation);
			detected2D3D.add( b );
		}
	}

	/**
	 * Given the mapping of 2D observations to known 3D points estimate the pose of the fiducial.
	 * This solves the P-n-P problem.
	 */
	protected boolean estimatePose( int which ,List points , Se3_F64 fiducialToCamera ) {
		return estimatePnP.process(points, initialEstimate) &&
				refinePnP.fitModel(points, initialEstimate, fiducialToCamera);
	}

	/**
	 * Returns a list of detected control points in the image for the specified fiducial.  Observations
	 * will be in distorted image pixels.
	 */
	public abstract List getDetectedControl(int which );

	/**
	 * 3D location of control points in the fiducial reference frame
	 * @return 3D location of control points
	 */
	protected abstract List getControl3D( int which );

	@Override
	public boolean is3D() {
		return hasCameraModel;
	}
}