boofcv.alg.fiducial.square.StabilitySquareFiducialEstimate Maven / Gradle / Ivy

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

import georegression.geometry.ConvertRotation3D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.shapes.Quadrilateral_F64;
import georegression.struct.so.Rodrigues_F64;
import org.ddogleg.struct.FastQueue;

/**
 * Used to estimate the stability of {@link BaseDetectFiducialSquare} fiducials.  Each corner point is disturbed by
 * adding the user provided "disturbance" independently to the x and y axis in the positive and negative direction.
 * The maximum resulting delta is then reported.  Maximum change in angle and orientation is computed.
 *
 * @author Peter Abeles
 */
public class StabilitySquareFiducialEstimate {

	// estimates the pose of the fiducial
	private QuadPoseEstimator estimator;
	// storage for corner observations
	private Quadrilateral_F64 work = new Quadrilateral_F64();

	// the pose estimate at the current unmodified location
	private Se3_F64 referenceCameraToWorld = new Se3_F64();

	// storage for the difference between a sample pose estimate and the reference
	private Se3_F64 difference = new Se3_F64();

	// storage for all the samples
	private FastQueue samples = new FastQueue(Se3_F64.class,true);

	private Rodrigues_F64 rodrigues = new Rodrigues_F64();

	// compute metrics.
	private double maxLocation;
	private double maxOrientation;

	public StabilitySquareFiducialEstimate(QuadPoseEstimator estimator) {
		this.estimator = estimator;
	}

	/**
	 * Processes the observation and generates a stability estimate
	 *
	 * @param sampleRadius Radius around the corner pixels it will sample
	 * @param input Observed corner location of the fiducial in distorted pixels.  Must be in correct order.
	 * @return true if successful or false if it failed
	 */
	public boolean process( double sampleRadius , Quadrilateral_F64 input ) {
		work.set(input);

		samples.reset();
		estimator.process(work);
		estimator.getWorldToCamera().invert(referenceCameraToWorld);

		samples.reset();

		createSamples(sampleRadius,work.a,input.a);
		createSamples(sampleRadius,work.b,input.b);
		createSamples(sampleRadius,work.c,input.c);
		createSamples(sampleRadius,work.d,input.d);

		if( samples.size() < 10 )
			return false;

		maxLocation = 0;
		maxOrientation = 0;
		for (int i = 0; i < samples.size(); i++) {
			referenceCameraToWorld.concat(samples.get(i), difference);

			ConvertRotation3D_F64.matrixToRodrigues(difference.getR(),rodrigues);

			double theta = Math.abs(rodrigues.theta);

			double d = difference.getT().norm();

			if( theta > maxOrientation ) {
				maxOrientation = theta;
			}
			if( d > maxLocation ) {
				maxLocation = d;
			}

		}

		return true;
	}

	/**
	 * Samples around the provided corner +- in x and y directions
	 */
	private void createSamples( double sampleRadius , Point2D_F64 workPoint , Point2D_F64 originalPoint ) {

		workPoint.x = originalPoint.x + sampleRadius;
		if( estimator.process(work) ) {
			samples.grow().set( estimator.getWorldToCamera() );
		}
		workPoint.x = originalPoint.x - sampleRadius;
		if( estimator.process(work) ) {
			samples.grow().set( estimator.getWorldToCamera() );
		}
		workPoint.x = originalPoint.x;

		workPoint.y = originalPoint.y + sampleRadius;
		if( estimator.process(work) ) {
			samples.grow().set( estimator.getWorldToCamera() );
		}
		workPoint.y = originalPoint.y - sampleRadius;
		if( estimator.process(work) ) {
			samples.grow().set( estimator.getWorldToCamera() );
		}
		workPoint.set(originalPoint);
	}

	public double getLocationStability() {
		return maxLocation;
	}

	public double getOrientationStability() {
		return maxOrientation;
	}
}