com.irurueta.ar.sfm.PlanarBestFundamentalMatrixEstimatorAndReconstructor Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of irurueta-ar Show documentation
Show all versions of irurueta-ar Show documentation
Augmented Reality and 3D reconstruction library
/*
* Copyright (C) 2017 Alberto Irurueta Carro ([email protected])
*
* 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 com.irurueta.ar.sfm;
import com.irurueta.algebra.Matrix;
import com.irurueta.ar.epipolar.Corrector;
import com.irurueta.ar.epipolar.CorrectorType;
import com.irurueta.ar.epipolar.FundamentalMatrix;
import com.irurueta.ar.epipolar.estimators.FundamentalMatrixEstimatorException;
import com.irurueta.ar.epipolar.estimators.PlanarFundamentalMatrixEstimator;
import com.irurueta.geometry.PinholeCamera;
import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
import com.irurueta.geometry.Point2D;
import com.irurueta.geometry.Point3D;
import com.irurueta.geometry.ProjectiveTransformation2D;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;
import com.irurueta.geometry.estimators.PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator;
import com.irurueta.geometry.estimators.PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
import com.irurueta.geometry.estimators.PointCorrespondenceProjectiveTransformation2DRobustEstimator;
import com.irurueta.geometry.estimators.RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
import com.irurueta.numerical.robust.InliersData;
import com.irurueta.numerical.robust.RobustEstimatorException;
import com.irurueta.numerical.robust.RobustEstimatorMethod;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.BitSet;
import java.util.List;
/**
* This class takes matched pairs of 2D points corresponding to a planar scene,
* estimates an homography relating both sets of points, decomposes such
* homography induced by the 3D plane on the scene, and uses such decomposition
* to determine the best epipolar geometry (e.g. fundamental matrix) by using
* the essential matrix and provided intrinsic camera parameters on both views
* corresponding to both sets of points to reconstruct points and choose the
* solution that produces the largest amount of points located in front of both
* cameras.
* This class requires 2 sets of matched 2D points and the intrinsic parameters
* of the cameras in both views, hence cameras must be calibrated in some way
* before using this class.
* This class is similar to PlanarFundamentalMatrixEstimator but picks the best
* solution by reconstructing the 3D points in the scene and choosing the
* solution that produces the largest amount of points located in front of both
* cameras.
*/
public class PlanarBestFundamentalMatrixEstimatorAndReconstructor {
/**
* Minimum number of matched points required to find a solution.
*/
public static final int MINIMUM_SIZE = 4;
/**
* List of matched 2D points in the left view.
*/
private List mLeftPoints;
/**
* List of matched 2D points in the right view.
*/
private List mRightPoints;
/**
* Intrinsic parameters for the camera on the left view.
*/
private PinholeCameraIntrinsicParameters mLeftIntrinsics;
/**
* Intrinsic parameters for the camera on the right view.
*/
private PinholeCameraIntrinsicParameters mRightIntrinsics;
/**
* Listener to attend events generated by this instance.
*/
private PlanarBestFundamentalMatrixEstimatorAndReconstructorListener
mListener;
/**
* Indicates whether this instance is locked while computing a solution.
*/
private boolean mLocked;
/**
* Homography estimator relating both views.
*/
private PointCorrespondenceProjectiveTransformation2DRobustEstimator
mHomographyEstimator;
/**
* Type of corrector to use to triangulate matched points using the
* corresponding essential matrix or null if no corrector needs to be used.
*/
private CorrectorType mEssentialCameraEstimatorCorrectorType =
Corrector.DEFAULT_TYPE;
/**
* Estimated homography.
*/
private ProjectiveTransformation2D mHomography;
/**
* Best estimated fundamental matrix.
*/
private FundamentalMatrix mFundamentalMatrix;
/**
* Best estimated triangulated points.
*/
private List mTriangulatedPoints;
/**
* Contains booleans indicating which of the best triangulated points are
* valid (i.e. lie in front of both estimated cameras) or not.
*/
private BitSet mValidTriangulatedPoints;
/**
* Best estimated camera for left view.
*/
private PinholeCamera mEstimatedLeftCamera;
/**
* Best estimated camera for right view.
*/
private PinholeCamera mEstimatedRightCamera;
/**
* Constructor.
*/
public PlanarBestFundamentalMatrixEstimatorAndReconstructor() {
mHomographyEstimator =
PointCorrespondenceProjectiveTransformation2DRobustEstimator.
create();
}
/**
* Constructor.
*
* @param leftPoints list of matched 2D points in the left view.
* @param rightPoints list of matched 2D points in the right view.
* @param leftIntrinsics intrinsic parameters for the camera on the left
* view.
* @param rightIntrinsics intrinsic parameters for the camera on the right
* view.
* @throws IllegalArgumentException if provided list of matched points do
* not contain enough points or if they have different sizes.
*/
public PlanarBestFundamentalMatrixEstimatorAndReconstructor(
final List leftPoints, final List rightPoints,
final PinholeCameraIntrinsicParameters leftIntrinsics,
final PinholeCameraIntrinsicParameters rightIntrinsics) {
this();
internalSetLeftAndRightPoints(leftPoints, rightPoints);
mLeftIntrinsics = leftIntrinsics;
mRightIntrinsics = rightIntrinsics;
}
/**
* Constructor.
*
* @param leftPoints list of matched 2D points in the left view.
* @param rightPoints list of matched 2D points in the right view.
* @param leftIntrinsics intrinsic parameters for the camera on the left
* view.
* @param rightIntrinsics intrinsic parameters for the camera on the right
* view.
* @param listener listener to be notified of events generated by this
* instance.
* @throws IllegalArgumentException if provided list of matched points do
* not contain enough points or if they have different sizes.
*/
public PlanarBestFundamentalMatrixEstimatorAndReconstructor(
final List leftPoints, final List rightPoints,
final PinholeCameraIntrinsicParameters leftIntrinsics,
final PinholeCameraIntrinsicParameters rightIntrinsics,
final PlanarBestFundamentalMatrixEstimatorAndReconstructorListener listener) {
this(leftPoints, rightPoints, leftIntrinsics, rightIntrinsics);
mListener = listener;
}
/**
* Gets list of matched 2D points in the left view.
*
* @return list of matched 2D points in the left view.
*/
public List getLeftPoints() {
return mLeftPoints;
}
/**
* Sets list of matched 2D points in the left view.
*
* @param leftPoints list of matched 2D points in the left view.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided points do not have enough
* points.
*/
public void setLeftPoints(final List leftPoints) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (leftPoints.size() < MINIMUM_SIZE) {
throw new IllegalArgumentException();
}
mLeftPoints = leftPoints;
}
/**
* Gets list of matched 2D points in the right view.
*
* @return list of matched 2D points in the right view.
*/
public List getRightPoints() {
return mRightPoints;
}
/**
* Sets list of matched 2D points in the right view.
*
* @param rightPoints list of matched 2D points in the right view.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided points do not have enough
* points.
*/
public void setRightPoints(final List rightPoints)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (rightPoints.size() < MINIMUM_SIZE) {
throw new IllegalArgumentException();
}
mRightPoints = rightPoints;
}
/**
* Sets lists of matched 2D points in the left and right views.
*
* @param leftPoints list of matched 2D points in the left view.
* @param rightPoints list of matched 2D points in the right view.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided points do not have enough
* points or lists have different sizes.
*/
public void setLeftAndRightPoints(final List leftPoints,
final List rightPoints) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
internalSetLeftAndRightPoints(leftPoints, rightPoints);
}
/**
* Gets intrinsic parameters for the camera on the left view.
*
* @return intrinsic parameters for the camera on the left view.
*/
public PinholeCameraIntrinsicParameters getLeftIntrinsics() {
return mLeftIntrinsics;
}
/**
* Sets intrinsic parameters for the camera on the left view.
*
* @param leftIntrinsics intrinsic parameters for the camera on the left
* view.
* @throws LockedException if estimator is locked.
*/
public void setLeftIntrinsics(
final PinholeCameraIntrinsicParameters leftIntrinsics)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mLeftIntrinsics = leftIntrinsics;
}
/**
* Gets intrinsic parameters for the camera on the right view.
*
* @return intrinsic parameters for the camera on the right view.
*/
public PinholeCameraIntrinsicParameters getRightIntrinsics() {
return mRightIntrinsics;
}
/**
* Sets intrinsic parameters for the camera on the right view.
*
* @param rightIntrinsics intrinsic parameters for the camera on the right
* view.
* @throws LockedException if estimator is locked.
*/
public void setRightIntrinsics(
final PinholeCameraIntrinsicParameters rightIntrinsics)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mRightIntrinsics = rightIntrinsics;
}
/**
* Gets internal homography estimator.
*
* @return internal homography estimator.
*/
public PointCorrespondenceProjectiveTransformation2DRobustEstimator getHomographyEstimator() {
return mHomographyEstimator;
}
/**
* Sets internal homography estimator.
*
* @param homographyEstimator internal homography estimator.
* @throws LockedException if estimator is locked.
* @throws NullPointerException if provided estimator is null.
*/
public void setHomographyEstimator(
final PointCorrespondenceProjectiveTransformation2DRobustEstimator homographyEstimator)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (homographyEstimator == null) {
throw new NullPointerException();
}
mHomographyEstimator = homographyEstimator;
}
/**
* Gets type of corrector to use to triangulate matched points using the
* corresponding essential matrix or null if no corrector needs to be used.
*
* @return corrector to use for triangulation or null.
*/
public CorrectorType getEssentialCameraEstimatorCorrectorType() {
return mEssentialCameraEstimatorCorrectorType;
}
/**
* Sets type of corrector to use to triangulate matched points using the
* corresponding essential matrix or null if no corrector needs to be used.
*
* @param correctorType corrector to use for triangulation or null.
* @throws LockedException if estimator is locked.
*/
public void setEssentialCameraEstimatorCorrectorType(
final CorrectorType correctorType) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mEssentialCameraEstimatorCorrectorType = correctorType;
}
/**
* Gets amount of confidence on homography estimation expressed as a value
* between 0.0 and 1.0 (which is equivalent to 100%). The amount of
* confidence indicates the probability that the estimated result is
* correct. Usually this value will be close to 1.0, but not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getHomographyConfidence() {
return mHomographyEstimator.getConfidence();
}
/**
* Sets amount of confidence on homography estimation expressed as a value
* between 0.0 and 1.0 (which is equivalent to 100%). The amount of
* confidence indicates the probability that the estimated result is
* correct. Usually this value will be close to 1.0, but not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and
* 1.0.
* @throws LockedException if estimator is locked.
*/
public void setHomographyConfidence(final double confidence)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mHomographyEstimator.setConfidence(confidence);
}
/**
* Returns maximum allowed number of iterations for homography estimation.
*
* @return maximum allowed number of iterations.
*/
public int getHomographyMaxIterations() {
return mHomographyEstimator.getMaxIterations();
}
/**
* Sets maximum allowed number of iterations for homography estimation.
*
* @param maxIterations maximum allowed number of iterations.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if estimator is locked.
*/
public void setHomographyMaxIterations(final int maxIterations)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mHomographyEstimator.setMaxIterations(maxIterations);
}
/**
* Indicates whether result of homography estimation is refined.
*
* @return true to refine homography result, false to simply use result
* found by robust estimation without further refining.
*/
public boolean isHomographyRefined() {
return mHomographyEstimator.isResultRefined();
}
/**
* Specifies whether homography estimation must be refined or not.
*
* @param refineResult true to refine homography result, false to simply use
* result found by robust estimator without further refining.
* @throws LockedException if estimator is locked.
*/
public void setHomographyRefined(final boolean refineResult)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mHomographyEstimator.setResultRefined(refineResult);
}
/**
* Indicates whether homography covariance must be kept after estimation.
* This setting is only taken into account if homography is refined.
*
* @return true if homography covariance must be kept after estimation,
* false otherwise.
*/
public boolean isHomographyCovarianceKept() {
return mHomographyEstimator.isCovarianceKept();
}
/**
* Specifies whether homography covariance must be kept after estimation.
* This setting is only taken into account if homography is refined.
*
* @param keepCovariance true if homography covariance must be kept after
* estimation, false otherwise.
* @throws LockedException if estimator is locked.
*/
public void setHomographyCovarianceKept(final boolean keepCovariance)
throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mHomographyEstimator.setCovarianceKept(keepCovariance);
}
/**
* Gets covariance for estimated homography if available.
* This is only available when homography has been refined and covariance is
* kept.
*
* @return estimated homography covariance or null.
*/
public Matrix getHomographyCovariance() {
return mHomographyEstimator.getCovariance();
}
/**
* Returns method being used for homography robust estimation.
*
* @return method being used for homography robust estimation.
*/
public RobustEstimatorMethod getHomographyMethod() {
return mHomographyEstimator.getMethod();
}
/**
* Returns quality scores corresponding to each pair of matched points.
* This is used for homography estimation.
* The larger the score value the better the quality of the matching.
*
* @return quality scores for each pair of matched points.
*/
public double[] getQualityScores() {
return mHomographyEstimator.getQualityScores();
}
/**
* Sets quality scores corresponding to each pair of matched points.
* This is used for homography estimation.
* The larger the score value the better the quality of the matching.
*
* @param qualityScore quality scores corresponding to eac pair of matched
* points.
* @throws LockedException if estimator is locked.
* @throws IllegalArgumentException if provided quality scores length is
* smaller than minimum required size (4 points).
*/
public void setQualityScores(final double[] qualityScore) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
mHomographyEstimator.setQualityScores(qualityScore);
}
/**
* Gets listener to attend events generated by this instance.
*
* @return listener to attend events generated by this instance.
*/
public PlanarBestFundamentalMatrixEstimatorAndReconstructorListener getListener() {
return mListener;
}
/**
* Sets listener to attend events generated by this instance.
*
* @param listener listener to attend events generated by this instance.
*/
public void setListener(
final PlanarBestFundamentalMatrixEstimatorAndReconstructorListener listener) {
mListener = listener;
}
/**
* Indicates whether this instance is locked while computing a solution.
*
* @return true if this instance is locked, false otherwise.
*/
public boolean isLocked() {
return mLocked;
}
/**
* Indicates whether this instance is ready to start the estimation when all
* required data has been provided.
*
* @return true if this instance is ready, false otherwise.
*/
public boolean isReady() {
return mLeftPoints != null && mLeftPoints.size() >= MINIMUM_SIZE &&
mRightPoints != null && mRightPoints.size() >= MINIMUM_SIZE &&
mLeftPoints.size() == mRightPoints.size() &&
mLeftIntrinsics != null && mRightIntrinsics != null &&
mHomographyEstimator.isReady();
}
/**
* Gets estimated homography.
*
* @return estimated homography.
*/
public ProjectiveTransformation2D getHomography() {
return mHomography;
}
/**
* Gets best estimated fundamental matrix.
*
* @return best estimated fundamental matrix.
*/
public FundamentalMatrix getFundamentalMatrix() {
return mFundamentalMatrix;
}
/**
* Gets best estimated triangulated points.
*
* @return best estimated triangulated points.
*/
public List getTriangulatedPoints() {
return mTriangulatedPoints;
}
/**
* Gets set containing booleans indicating which of the best triangulated
* points are valid (i.e. lie in front of both estimated cameras) or not.
*
* @return set indicating which of the best triangulated points are valid.
*/
public BitSet getValidTriangulatedPoints() {
return mValidTriangulatedPoints;
}
/**
* Gets best estimated camera for left view.
*
* @return best estimated camera for left view.
*/
public PinholeCamera getEstimatedLeftCamera() {
return mEstimatedLeftCamera;
}
/**
* Gets best estimated camera for right view.
*
* @return best estimated camera for right view.
*/
public PinholeCamera getEstimatedRightCamera() {
return mEstimatedRightCamera;
}
/**
* Estimates homography, best fundamental matrix, their cameras and
* reconstructs matched points.
*
* @throws LockedException if estimator is locked.
* @throws NotReadyException if estimator is not ready because required data
* is missing.
* @throws FundamentalMatrixEstimatorException if something fails, typically
* due to numerical instabilities.
*/
public void estimateAndReconstruct() throws LockedException,
NotReadyException, FundamentalMatrixEstimatorException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
// always enable inlier estimation for homography
enableHomographyInliersEstimation();
try {
mLocked = true;
if (mListener != null) {
mListener.onEstimateStart(this);
}
// estimate homography
mHomography = mHomographyEstimator.estimate();
final InliersData homographyInliers =
mHomographyEstimator.getInliersData();
// estimate all fundamental matrices for homography
final PlanarFundamentalMatrixEstimator fundamentalMatrixEstimator =
new PlanarFundamentalMatrixEstimator(mHomography,
mLeftIntrinsics, mRightIntrinsics);
final List fundamentalMatrices =
fundamentalMatrixEstimator.estimate();
if (fundamentalMatrices == null) {
throw new FundamentalMatrixEstimatorException();
}
// select homography inlier points
final List leftPoints = new ArrayList<>();
final List rightPoints = new ArrayList<>();
final BitSet bitset = homographyInliers.getInliers();
final int bitsetLength = bitset.length();
for (int i = 0; i < bitsetLength; i++) {
if (bitset.get(i)) {
// is inlier
leftPoints.add(mLeftPoints.get(i));
rightPoints.add(mRightPoints.get(i));
}
}
// pick best fundamental matrix
final EssentialMatrixInitialCamerasEstimator essentialCamerasEstimator =
new EssentialMatrixInitialCamerasEstimator(mLeftIntrinsics,
mRightIntrinsics, leftPoints, rightPoints);
essentialCamerasEstimator.setCorrectorType(
mEssentialCameraEstimatorCorrectorType);
essentialCamerasEstimator.setPointsTriangulated(true);
essentialCamerasEstimator.setValidTriangulatedPointsMarked(true);
int numBest = 0;
for (final FundamentalMatrix fundamentalMatrix : fundamentalMatrices) {
essentialCamerasEstimator.setFundamentalMatrix(
fundamentalMatrix);
essentialCamerasEstimator.estimate();
final BitSet validPoints = essentialCamerasEstimator.
getValidTriangulatedPoints();
final int num = validPoints.cardinality();
if (num > numBest) {
numBest = num;
mFundamentalMatrix = fundamentalMatrix;
mTriangulatedPoints =
essentialCamerasEstimator.getTriangulatedPoints();
mValidTriangulatedPoints = validPoints;
mEstimatedLeftCamera =
essentialCamerasEstimator.getEstimatedLeftCamera();
mEstimatedRightCamera =
essentialCamerasEstimator.getEstimatedRightCamera();
}
}
if (mListener != null) {
mListener.onEstimateEnd(this);
}
} catch (final RobustEstimatorException | InitialCamerasEstimationFailedException e) {
throw new FundamentalMatrixEstimatorException(e);
} finally {
mLocked = false;
}
}
/**
* Internal method that sets list of matched 2D points in the left and right
* views.
*
* @param leftPoints list of matched 2D points in the left view.
* @param rightPoints list of matched 2D points in the right view.
* @throws IllegalArgumentException if provided points do not have enough
* points or lists have different sizes.
*/
private void internalSetLeftAndRightPoints(final List leftPoints,
final List rightPoints) {
if (leftPoints.size() < MINIMUM_SIZE ||
rightPoints.size() < MINIMUM_SIZE ||
leftPoints.size() != rightPoints.size()) {
throw new IllegalArgumentException();
}
mLeftPoints = leftPoints;
mRightPoints = rightPoints;
try {
mHomographyEstimator.setPoints(leftPoints, rightPoints);
if (mHomographyEstimator.getMethod() == RobustEstimatorMethod.PROMedS) {
final PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator promedsEstimator =
(PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator) mHomographyEstimator;
if (promedsEstimator.getQualityScores() == null) {
final double[] qualityScores = new double[leftPoints.size()];
Arrays.fill(qualityScores, 1.0);
promedsEstimator.setQualityScores(qualityScores);
}
} else if (mHomographyEstimator.getMethod() == RobustEstimatorMethod.PROSAC) {
final PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator prosacEstimator =
(PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) mHomographyEstimator;
if (prosacEstimator.getQualityScores() == null) {
final double[] qualityScores = new double[leftPoints.size()];
Arrays.fill(qualityScores, 1.0);
prosacEstimator.setQualityScores(qualityScores);
}
}
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Ensures that inlier estimation is enabled on homography estimator.
*/
private void enableHomographyInliersEstimation() {
try {
if (mHomographyEstimator.getMethod() == RobustEstimatorMethod.RANSAC) {
final RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator ransacHomographyEstimator =
(RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) mHomographyEstimator;
ransacHomographyEstimator.setComputeAndKeepInliersEnabled(true);
ransacHomographyEstimator.setComputeAndKeepResidualsEnabled(true);
} else if (mHomographyEstimator.getMethod() == RobustEstimatorMethod.PROSAC) {
final PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator prosacHomographyEstimator =
(PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) mHomographyEstimator;
prosacHomographyEstimator.setComputeAndKeepInliersEnabled(true);
prosacHomographyEstimator.setComputeAndKeepResidualsEnabled(true);
}
} catch (final LockedException ignore) {
// never happens
}
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy