boofcv.factory.geo.FactoryMultiViewRobust Maven / Gradle / Ivy

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

import boofcv.abst.geo.*;
import boofcv.abst.geo.fitting.DistanceFromModelResidual;
import boofcv.abst.geo.fitting.GenerateEpipolarMatrix;
import boofcv.abst.geo.fitting.ModelManagerEpipolarMatrix;
import boofcv.alg.geo.f.FundamentalResidualSampson;
import boofcv.alg.geo.pose.PnPDistanceReprojectionSq;
import boofcv.alg.geo.robust.*;
import boofcv.alg.geo.selfcalib.DistanceMetricTripleReprojection23;
import boofcv.alg.geo.selfcalib.MetricCameraTriple;
import boofcv.alg.geo.selfcalib.ModelManagerMetricCameraTriple;
import boofcv.concurrency.BoofConcurrency;
import boofcv.struct.calib.ElevateViewInfo;
import boofcv.struct.geo.*;
import georegression.fitting.homography.ModelManagerHomography2D_F64;
import georegression.fitting.se.ModelManagerSe3_F64;
import georegression.struct.homography.Homography2D_F64;
import georegression.struct.se.Se3_F64;
import org.ddogleg.fitting.modelset.DistanceFromModel;
import org.ddogleg.fitting.modelset.ModelManager;
import org.ddogleg.fitting.modelset.ModelMatcher;
import org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares;
import org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares_MT;
import org.ddogleg.fitting.modelset.ransac.Ransac;
import org.ddogleg.fitting.modelset.ransac.Ransac_MT;
import org.ddogleg.struct.Factory;
import org.ejml.data.DMatrixRMaj;
import org.jetbrains.annotations.Nullable;

/**
 * Factory for creating robust false-positive tolerant estimation algorithms in multi-view geometry. These
 * algorithms tend to have a lot of boilerplate associated with them and the goal of this factory
 * is to make their use much easier and less error prone.
 *
 * @author Peter Abeles
 */
public class FactoryMultiViewRobust {

	/**
	 * Robust solution to PnP problem using {@link LeastMedianOfSquares LMedS}. Input observations are
	 * in normalized image coordinates.
	 *
	 * 
	 *     Input observations are in normalized image coordinates NOT pixels
	 *     Error units are pixels squared.
	 * 
	 *
	 * See code for all the details.
	 *
	 * @param configPnP PnP parameters. Can't be null.
	 * @param configLMedS Parameters for LMedS. Can't be null.
	 * @return Robust Se3_F64 estimator
	 */
	public static ModelMatcherMultiview pnpLMedS( @Nullable ConfigPnP configPnP,
																	  ConfigLMedS configLMedS ) {
		if (configPnP == null)
			configPnP = new ConfigPnP();
		configPnP.checkValidity();
		configLMedS.checkValidity();

		var manager = new ModelManagerSe3_F64();

		var lmeds = new LeastMedianOfSquaresCalibrated<>(createLMEDS(configLMedS, manager, Point2D3D.class));
		lmeds.getFitter().setErrorFraction(configLMedS.errorFraction);

		ConfigPnP _configPnP = configPnP;
		lmeds.setModel(() -> {
			Estimate1ofPnP estimatorPnP = FactoryMultiView.pnp_1(
					_configPnP.which, _configPnP.epnpIterations, _configPnP.numResolve);
			return new EstimatorToGenerator<>(estimatorPnP);
		}, PnPDistanceReprojectionSq::new);
		return lmeds;
	}

	/**
	 * Robust solution to PnP problem using {@link Ransac}. Input observations are in normalized
	 * image coordinates. Found transform is from world to camera.
	 *
	 * NOTE: Observations are in normalized image coordinates NOT pixels.
	 *
	 * See code for all the details.
	 *
	 * @param configPnP PnP parameters. Can't be null.
	 * @param configRansac Parameters for RANSAC. Can't be null.
	 * @return Robust Se3_F64 estimator
	 * @see Estimate1ofPnP
	 */
	public static RansacCalibrated pnpRansac( @Nullable ConfigPnP configPnP,
																  ConfigRansac configRansac ) {
		if (configPnP == null)
			configPnP = new ConfigPnP();
		configPnP.checkValidity();
		configRansac.checkValidity();

		// convert from pixels to pixels squared
		double threshold = configRansac.inlierThreshold*configRansac.inlierThreshold;

		var ransac = new RansacCalibrated<>(createRansac(
				configRansac, threshold,  new ModelManagerSe3_F64(), Point2D3D.class));

		ConfigPnP _configPnP = configPnP;
		ransac.setModel(() -> {
			Estimate1ofPnP estimatorPnP = FactoryMultiView.pnp_1(
					_configPnP.which, _configPnP.epnpIterations, _configPnP.numResolve);
			return new EstimatorToGenerator<>(estimatorPnP);
		}, PnPDistanceReprojectionSq::new);

		return ransac;
	}

	/**
	 * Robust solution for estimating {@link Se3_F64} using epipolar geometry from two views with
	 * {@link LeastMedianOfSquares LMedS}. Input observations are in normalized image coordinates.
	 *
	 * 
	 *     Error units is pixels squared times two
	 * 
	 *
	 * See code for all the details.
	 *
	 * @param configEssential Essential matrix estimation parameters. Can't be null.
	 * @param configLMedS Parameters for RANSAC. Can't be null.
	 * @return Robust Se3_F64 estimator
	 */
	public static ModelMatcherMultiview
	baselineLMedS( @Nullable ConfigEssential configEssential, ConfigLMedS configLMedS ) {
		if (configEssential == null)
			configEssential = new ConfigEssential();
		configEssential.checkValidity();
		configLMedS.checkValidity();

		var manager = new ModelManagerSe3_F64();
		var lmeds = new LeastMedianOfSquaresCalibrated<>(createLMEDS(configLMedS, manager, AssociatedPair.class));
		lmeds.getFitter().setErrorFraction(configLMedS.errorFraction);

		ConfigEssential _configEssential = configEssential;
		lmeds.setModel(() -> {
			Estimate1ofEpipolar epipolar = FactoryMultiView.
					essential_1(_configEssential.which, _configEssential.numResolve);
			Triangulate2ViewsMetricH triangulate = FactoryMultiView.triangulate2ViewMetricH(
					new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
			return new Se3FromEssentialGenerator(epipolar, triangulate);
		}, () -> {
			Triangulate2ViewsMetricH triangulate = FactoryMultiView.triangulate2ViewMetricH(
					new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
			return new DistanceSe3SymmetricSq(triangulate);
		});


		return lmeds;
	}

	public static ModelMatcher
	fundamentalLMedS( @Nullable ConfigFundamental configFundamental, ConfigLMedS configLMedS ) {
		if (configFundamental == null)
			configFundamental = new ConfigFundamental();

		configFundamental.checkValidity();
		configLMedS.checkValidity();

		ConfigFundamental _configFundamental = configFundamental;

		var manager = new ModelManagerEpipolarMatrix();

		LeastMedianOfSquares alg = createLMEDS(configLMedS, manager, AssociatedPair.class);
		alg.setModel(
				() -> {
					Estimate1ofEpipolar estimateF = FactoryMultiView.fundamental_1(_configFundamental.which,
							_configFundamental.numResolve);
					return new GenerateEpipolarMatrix(estimateF);
				},
				() -> switch (_configFundamental.errorModel) {
					case SAMPSON -> new DistanceFromModelResidual<>(new FundamentalResidualSampson());
					case GEOMETRIC -> new DistanceFundamentalGeometric();
				});

		return alg;
	}

	/**
	 * Robust solution for estimating the stereo baseline {@link Se3_F64} using epipolar geometry from two views with
	 * {@link RansacCalibrated}. Input observations are in normalized image coordinates.
	 *
	 * See code for all the details.
	 *
	 * @param configEssential Essential matrix estimation parameters.
	 * @param configRansac Parameters for RANSAC. Can't be null.
	 * @return Robust Se3_F64 estimator
	 */
	public static RansacCalibrated
	baselineRansac( @Nullable ConfigEssential configEssential, ConfigRansac configRansac ) {
		if (configEssential == null)
			configEssential = new ConfigEssential();
		configEssential.checkValidity();
		configRansac.checkValidity();

		if (configEssential.errorModel != ConfigEssential.ErrorModel.GEOMETRIC) {
			throw new RuntimeException("Error model has to be Euclidean");
		}

		ConfigEssential _configEssential = configEssential;

		double ransacTOL = configRansac.inlierThreshold*configRansac.inlierThreshold*2.0;

		var ransac = new RansacCalibrated<>(createRansac(
				configRansac, ransacTOL, new ModelManagerSe3_F64(), AssociatedPair.class));

		ransac.setModel(() -> {
			Estimate1ofEpipolar epipolar = FactoryMultiView.
					essential_1(_configEssential.which, _configEssential.numResolve);
			Triangulate2ViewsMetricH triangulate = FactoryMultiView.triangulate2ViewMetricH(
					new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
			return new Se3FromEssentialGenerator(epipolar, triangulate);
		}, () -> {
			Triangulate2ViewsMetricH triangulate = FactoryMultiView.triangulate2ViewMetricH(
					new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
			return new DistanceSe3SymmetricSq(triangulate);
		});

		return ransac;
	}

	/**
	 * Robust solution for estimating the stereo baseline {@link Se3_F64} using epipolar geometry from two views with
	 * {@link RansacCalibrated2}. Input observations are in pointing vector coordinates.
	 *
	 * See code for all the details.
	 *
	 * @param configEssential Essential matrix estimation parameters.
	 * @param configRansac Parameters for RANSAC. Can't be null.
	 * @return Robust Se3_F64 estimator
	 */
	public static RansacCalibrated2
	baselinePointingRansac( @Nullable ConfigEssential configEssential, ConfigRansac configRansac ) {
		if (configEssential == null)
			configEssential = new ConfigEssential();
		configEssential.checkValidity();
		configRansac.checkValidity();

		if (configEssential.errorModel != ConfigEssential.ErrorModel.GEOMETRIC) {
			throw new RuntimeException("Error model has to be Euclidean");
		}

		Estimate1ofEpipolarPointing epipolar = FactoryMultiView.
				essentialPointing_1(configEssential.which, configEssential.numResolve);

		Triangulate2PointingMetricH triangulate = FactoryMultiView.triangulate2PointingMetricH(
				new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
		var manager = new ModelManagerSe3_F64();
		var generateEpipolarMotion = new Se3FromEssentialPointingGenerator(epipolar, triangulate);

		var distanceSe3 = new DistanceSe3SymmetricSqPointing(triangulate);

		double ransacTOL = configRansac.inlierThreshold*configRansac.inlierThreshold*2.0;

		return new RansacCalibrated2<>(configRansac.randSeed, configRansac.iterations, ransacTOL,
				manager, generateEpipolarMotion, distanceSe3);
	}

	/**
	 * Estimates the essential matrix given associated image features in normalized image coordinates using RANSAC.
	 *
	 * @param configEssential (Optional) Configuration for computing essential matrix
	 * @param configRansac Configuration for RANSAC
	 * @return Robust essential matrix estimator
	 */
	public static ModelMatcherMultiview
	essentialRansac( @Nullable ConfigEssential configEssential, ConfigRansac configRansac ) {
		if (configEssential == null)
			configEssential = new ConfigEssential();
		configEssential.checkValidity();
		configRansac.checkValidity();

		if (configEssential.errorModel == ConfigEssential.ErrorModel.GEOMETRIC) {
			// The best error has been selected. Compute using the baseline algorithm then convert back into
			// essential matrix
			return new MmmvSe3ToEssential(baselineRansac(configEssential, configRansac));
		}

		double ransacTOL = configRansac.inlierThreshold*configRansac.inlierThreshold;

		RansacCalibrated ransac = new RansacCalibrated(createRansac(
				configRansac, ransacTOL, new ModelManagerEpipolarMatrix(), AssociatedPair.class));

		ConfigEssential _configEssential = configEssential;
		ransac.setModel(() -> {
					Estimate1ofEpipolar estimateF = FactoryMultiView.essential_1(_configEssential.which,
							_configEssential.numResolve);
					return new GenerateEpipolarMatrix(estimateF);
				},
				DistanceMultiView_EssentialSampson::new);

		return ransac;
	}

	public static ModelMatcher fundamentalRansac(
			ConfigFundamental configFundamental,
			ConfigRansac configRansac ) {

		configFundamental.checkValidity();
		configRansac.checkValidity();

		ModelManager manager = new ModelManagerEpipolarMatrix();

		double ransacTol = configRansac.inlierThreshold*configRansac.inlierThreshold;

		Ransac ransac =
				createRansac(configRansac, ransacTol, manager, AssociatedPair.class);

		ransac.setModel(
				() -> {
					Estimate1ofEpipolar estimateF = FactoryMultiView.fundamental_1(configFundamental.which,
							configFundamental.numResolve);
					return new GenerateEpipolarMatrix(estimateF);
				},
				() -> switch (configFundamental.errorModel) {
					case SAMPSON -> new DistanceFromModelResidual<>(new FundamentalResidualSampson());
					case GEOMETRIC -> new DistanceFundamentalGeometric();
				});
		return ransac;
	}

	/**
	 * Robust solution for estimating {@link Homography2D_F64} with {@link LeastMedianOfSquares LMedS}. Input
	 * observations are in pixel coordinates.
	 *
	 * 
	 *     Four point linear is used internally
	 *     
inlierThreshold is in pixels
	 * 
	 *
	 * See code for all the details.
	 *
	 * @param configHomography Homography estimation parameters. If null default is used.
	 * @param configLMedS Parameters for LMedS. Can't be null.
	 * @return Homography estimator
	 */
	public static LeastMedianOfSquares
	homographyLMedS( @Nullable ConfigHomography configHomography, ConfigLMedS configLMedS ) {
		if (configHomography == null)
			configHomography = new ConfigHomography();
		configHomography.checkValidity();
		configLMedS.checkValidity();

		ConfigHomography _configHomography = configHomography;

		var manager = new ModelManagerHomography2D_F64();

		LeastMedianOfSquares lmeds =
				createLMEDS(configLMedS, manager, AssociatedPair.class);

		lmeds.setModel(
				() -> new GenerateHomographyLinear(_configHomography.normalize),
				DistanceHomographySq::new);
		return lmeds;
	}

	/**
	 * Robust solution for estimating {@link Homography2D_F64} with {@link Ransac}. Input
	 * observations are in pixel coordinates.
	 *
	 * 
	 *     Four point linear is used internally
	 *     
inlierThreshold is in pixels
	 * 
	 *
	 * See code for all the details.
	 *
	 * @param configHomography Homography estimation parameters. If null default is used.
	 * @param configRansac Parameters for RANSAC. Can't be null.
	 * @return Homography estimator
	 */
	public static Ransac
	homographyRansac( @Nullable ConfigHomography configHomography, ConfigRansac configRansac ) {
		if (configHomography == null)
			configHomography = new ConfigHomography();
		configHomography.checkValidity();
		configRansac.checkValidity();

		ConfigHomography _configHomography = configHomography;

		ModelManager manager = new ModelManagerHomography2D_F64();

		double ransacTol = configRansac.inlierThreshold*configRansac.inlierThreshold;
		Ransac ransac =
				createRansac(configRansac, ransacTol, manager, AssociatedPair.class);
		ransac.setModel(
				() -> new GenerateHomographyLinear(_configHomography.normalize),
				DistanceHomographySq::new);
		return ransac;
	}

	/**
	 * Estimates a homography from normalized image coordinates but computes the error in pixel coordinates
	 *
	 * @param configRansac RANSAC configuration
	 * @return Ransac
	 * @see GenerateHomographyLinear
	 * @see DistanceHomographyCalibratedSq
	 */
	public static RansacCalibrated
	homographyCalibratedRansac( ConfigRansac configRansac ) {
		configRansac.checkValidity();

		double ransacTol = configRansac.inlierThreshold*configRansac.inlierThreshold;

		var ransac = new RansacCalibrated<>(createRansac(
				configRansac, ransacTol, new ModelManagerHomography2D_F64(), AssociatedPair.class));
		ransac.setModel(() ->
				new GenerateHomographyLinear(false), DistanceHomographyCalibratedSq::new);

		return ransac;
	}

	/**
	 * Robust RANSAC based estimator for
	 *
	 * @param configTrifocal Configuration for trifocal tensor calculation
	 * @param configError Configuration for how trifocal error is computed
	 * @param configRansac Configuration for RANSAC
	 * @return RANSAC
	 * @see FactoryMultiView#trifocal_1
	 */
	public static Ransac
	trifocalRansac( @Nullable ConfigTrifocal configTrifocal,
					@Nullable ConfigTrifocalError configError,
					ConfigRansac configRansac ) {
		if (configTrifocal == null)
			configTrifocal = new ConfigTrifocal();
		if (configError == null)
			configError = new ConfigTrifocalError();

		configTrifocal.checkValidity();
		configError.checkValidity();
		configRansac.checkValidity();

		// needed for lambdas
		final ConfigTrifocal _configTrifocal = configTrifocal;
		final ConfigTrifocalError _configError = configError;

		double ransacTol;
		Factory> distance;

		switch (configError.model) {
			case REPROJECTION -> {
				ransacTol = 3.0*configRansac.inlierThreshold*configRansac.inlierThreshold;
				distance = DistanceTrifocalReprojectionSq::new;
			}
			case REPROJECTION_REFINE -> {
				ransacTol = 3.0*configRansac.inlierThreshold*configRansac.inlierThreshold;
				distance = () -> new DistanceTrifocalReprojectionSq(
						_configError.converge.gtol, _configError.converge.maxIterations);
			}
			case POINT_TRANSFER -> {
				ransacTol = 2.0*configRansac.inlierThreshold*configRansac.inlierThreshold;
				distance = DistanceTrifocalTransferSq::new;
			}
			default -> throw new IllegalArgumentException("Unknown error model " + configError.model);
		}

		ModelManager manager = new ManagerTrifocalTensor();

		Ransac ransac =
				createRansac(configRansac, ransacTol, manager, AssociatedTriple.class);

		ransac.setModel(
				() -> new GenerateTrifocalTensor(FactoryMultiView.trifocal_1(_configTrifocal)),
				distance);
		return ransac;
	}

	/**
	 * Projective to metric self calibration from 3-views
	 *
	 * @param configSelfcalib (Input) configuration for self calibration
	 * @param configRansac (Input) configuration for RANSAC
	 * @return RANSAC
	 */
	public static RansacProjective
	metricThreeViewRansac( @Nullable ConfigPixelsToMetric configSelfcalib,
						   ConfigRansac configRansac ) {
		configRansac.checkValidity();

		// Pixel error squared in two views
		double ransacTol = configRansac.inlierThreshold*configRansac.inlierThreshold*2;

		// lint:forbidden ignore_below 1
		var generator = FactoryMultiView.selfCalibThree(configSelfcalib);
		var manager = new ModelManagerMetricCameraTriple();
		var distance = new DistanceFromModelIntoViews
				(new DistanceMetricTripleReprojection23(), 3);

		return new RansacProjective<>(configRansac.randSeed, manager, generator, distance, configRansac.iterations, ransacTol);
	}

	public static LeastMedianOfSquaresProjective
	metricThreeViewLmeds( @Nullable ConfigPixelsToMetric configSelfcalib,
						  ConfigLMedS configLMedS ) {
		configLMedS.checkValidity();

		// lint:forbidden ignore_below 1
		var generator = FactoryMultiView.selfCalibThree(configSelfcalib);
		var manager = new ModelManagerMetricCameraTriple();
		var distance = new DistanceFromModelIntoViews
				(new DistanceMetricTripleReprojection23(), 3);

		return new LeastMedianOfSquaresProjective<>(configLMedS.randSeed, configLMedS.totalCycles, Double.MAX_VALUE,
				configLMedS.errorFraction, manager, generator, distance);
	}

	public static  LeastMedianOfSquares
	createLMEDS( ConfigLMedS configLMedS, ModelManager manager, Class pointType ) {
		LeastMedianOfSquares alg = BoofConcurrency.isUseConcurrent() ?
				new LeastMedianOfSquares_MT<>(configLMedS.randSeed, configLMedS.totalCycles, manager, pointType)
				:
				new LeastMedianOfSquares<>(configLMedS.randSeed, configLMedS.totalCycles, manager, pointType);
		alg.setErrorFraction(configLMedS.errorFraction);
		return alg;
	}

	/**
	 * Returns a new instance of RANSAC. If concurrency is turned on then a concurrent version will be returned.
	 *
	 * @param ransacTol inlier tolerance. The tolerance on config isn't used since that might have the wrong units. This
	 * lets the user easily adjust the units without modifying the config.
	 */
	public static  Ransac
	createRansac( ConfigRansac configRansac, double ransacTol, ModelManager manager, Class pointType ) {
		return BoofConcurrency.isUseConcurrent() ?
				new Ransac_MT<>(configRansac.randSeed, configRansac.iterations, ransacTol, manager, pointType)
				:
				new Ransac<>(configRansac.randSeed, configRansac.iterations, ransacTol, manager, pointType);
	}
}