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

Go to download
/*
 * Copyright (c) 2021, 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.DogArray;

/**
 * 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 final QuadPoseEstimator estimator;
	// storage for corner observations
	private final Quadrilateral_F64 work = new Quadrilateral_F64();

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

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

	// storage for all the samples
	private final DogArray samples = new DogArray<>(Se3_F64::new);

	private final 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.setTo(input);

		samples.reset();
		estimator.process(work, false);
		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, false)) {
			samples.grow().setTo(estimator.getWorldToCamera());
		}
		workPoint.x = originalPoint.x - sampleRadius;
		if (estimator.process(work, false)) {
			samples.grow().setTo(estimator.getWorldToCamera());
		}
		workPoint.x = originalPoint.x;

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

	public double getLocationStability() {
		return maxLocation;
	}

	public double getOrientationStability() {
		return maxOrientation;
	}
}