BasePairedViewsSparseReconstructor.java
/*
* Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
*
* 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.AlgebraException;
import com.irurueta.ar.calibration.estimators.LMSEImageOfAbsoluteConicEstimator;
import com.irurueta.ar.epipolar.Corrector;
import com.irurueta.ar.epipolar.EpipolarException;
import com.irurueta.ar.epipolar.EssentialMatrix;
import com.irurueta.ar.epipolar.FundamentalMatrix;
import com.irurueta.ar.epipolar.estimators.EightPointsFundamentalMatrixEstimator;
import com.irurueta.ar.epipolar.estimators.FundamentalMatrixEstimatorMethod;
import com.irurueta.ar.epipolar.estimators.FundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.LMedSFundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.MSACFundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.PROMedSFundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.PROSACFundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.RANSACFundamentalMatrixRobustEstimator;
import com.irurueta.ar.epipolar.estimators.SevenPointsFundamentalMatrixEstimator;
import com.irurueta.geometry.*;
import com.irurueta.geometry.estimators.LMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator;
import com.irurueta.geometry.estimators.MSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
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.ProjectiveTransformation2DRobustEstimator;
import com.irurueta.geometry.estimators.RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
import java.util.ArrayList;
import java.util.List;
/**
* Base class in charge of estimating cameras and 3D reconstructed points from sparse
* image point correspondences in pairs of views.
* Views are processed in pairs so that fundamental matrix is estimated and pairs of
* cameras and reconstructed points are computed.
* Because view pairs are processed separately, the scale of each view pair is
* estimated individually, hence the scale will need to be
*
* @param <C> type of configuration.
* @param <R> type of re-constructor.
* @param <L> type of listener.
*/
@SuppressWarnings("DuplicatedCode")
public abstract class BasePairedViewsSparseReconstructor<
C extends BasePairedViewsSparseReconstructorConfiguration<C>,
R extends BasePairedViewsSparseReconstructor<C, R, L>,
L extends BasePairedViewsSparseReconstructorListener<R>> {
/**
* Minimum required number of views.
*/
public static final int MIN_NUMBER_OF_VIEWS = 2;
/**
* Default scale.
*/
protected static final double DEFAULT_SCALE = 1.0;
/**
* Current estimated camera in a metric stratum (i.e. up to scale).
*/
protected EstimatedCamera currentMetricEstimatedCamera;
/**
* Previous estimated camera in a metric stratum (i.e. up to scale).
*/
protected EstimatedCamera previousMetricEstimatedCamera;
/**
* Reconstructed 3D points for current pair of views in a metric stratum (i.e. up to scale).
*/
protected List<ReconstructedPoint3D> metricReconstructedPoints;
/**
* Transformation to set reference frame on estimated pair of Euclidean cameras.
* This is used when estimating a new pair of Euclidean cameras to transform such pair to
* the location and rotation of the last estimated Euclidean camera so that the first camera
* of the pair is not referred to the world origin.
*/
protected MetricTransformation3D referenceEuclideanTransformation;
/**
* Current estimated scale. This will typically converge to a constant value as more views are
* processed.
* The smaller the variance of estimated scale, the more accurate the scale will be.
*/
protected double currentScale = DEFAULT_SCALE;
/**
* Current estimated camera in euclidean stratum (i.e. with actual scale).
*/
protected EstimatedCamera currentEuclideanEstimatedCamera;
/**
* Previous estimated camera in Euclidean stratum (i.e. with actual scale).
*/
protected EstimatedCamera previousEuclideanEstimatedCamera;
/**
* Reconstructed 3D points for current pair of views in Euclidean stratum (i.e. with actual
* scale).
*/
protected List<ReconstructedPoint3D> euclideanReconstructedPoints;
/**
* Configuration for this re-constructor.
*/
protected C configuration;
/**
* Listener in charge of handling events such as when reconstruction starts,
* ends, when certain data is needed or when estimation of data has been
* computed.
*/
protected L listener;
/**
* Indicates whether reconstruction has failed or not.
*/
protected volatile boolean failed;
/**
* Indicates whether reconstruction is running or not.
*/
protected volatile boolean running;
/**
* ID of previous view.
*/
protected int previousViewId = 0;
/**
* ID of current view.
*/
protected int currentViewId;
/**
* Center of current Euclidean camera on last view pair.
*/
protected Point3D lastEuclideanCameraCenter = new InhomogeneousPoint3D();
/**
* Rotation of current Euclidean camera on last view pair.
*/
protected Rotation3D lastEuclideanCameraRotation;
/**
* Center of current metric camera on last view pair.
*/
private Point3D lastMetricCameraCenter;
/**
* Rotation of current metric camera on last view pair.
*/
private Rotation3D mLastMetricCameraRotation;
/**
* Inverse metric camera rotation. This is reused for memory efficiency.
*/
private Rotation3D invMetricCameraRotation;
/**
* Current estimated fundamental matrix.
*/
private EstimatedFundamentalMatrix currentEstimatedFundamentalMatrix;
/**
* Indicates whether reconstruction has been cancelled or not.
*/
private volatile boolean cancelled;
/**
* Counter of number of processed views.
*/
private int viewCount;
/**
* Indicates whether reconstruction has finished or not.
*/
private boolean finished = false;
/**
* Samples on previous view.
*/
private List<Sample2D> previousViewSamples;
/**
* Samples on last processed view (i.e. current view).
*/
private List<Sample2D> currentViewSamples;
/**
* Matches between first and current view.
* Views are always processed in pairs.
*/
private final List<MatchedSamples> matches = new ArrayList<>();
/**
* Transformation to set reference frame on estimated pair of metric cameras.
* This is used when estimating a new pair of metric cameras to transform such pair to
* the location and rotation of last estimated metric camera so that the first camera of
* the pair is not referred to the world origin.
*/
private EuclideanTransformation3D referenceMetricTransformation;
/**
* Constructor.
*
* @param configuration configuration for this re-constructor.
* @param listener listener in charge of handling events.
* @throws NullPointerException if listener or configuration is not
* provided.
*/
protected BasePairedViewsSparseReconstructor(final C configuration, final L listener) {
if (configuration == null || listener == null) {
throw new NullPointerException();
}
this.configuration = configuration;
this.listener = listener;
}
/**
* Gets configuration for this re-constructor.
*
* @return configuration for this reconstructor.
*/
public C getConfiguration() {
return configuration;
}
/**
* Gets listener in charge of handling events such as when reconstruction
* starts, ends, when certain data is needed or when estimation of data has
* been computed.
*
* @return listener in charge of handling events.
*/
public L getListener() {
return listener;
}
/**
* Indicates whether reconstruction is running or not.
*
* @return true if reconstruction is running, false if reconstruction has
* stopped for any reason.
*/
public boolean isRunning() {
return running;
}
/**
* Indicates whether reconstruction has been cancelled or not.
*
* @return true if reconstruction has been cancelled, false otherwise.
*/
public boolean isCancelled() {
return cancelled;
}
/**
* Indicates whether reconstruction has failed or not.
*
* @return true if reconstruction has failed, false otherwise.
*/
public boolean hasFailed() {
return failed;
}
/**
* Indicates whether the reconstruction has finished.
*
* @return true if reconstruction has finished, false otherwise.
*/
public boolean isFinished() {
return finished;
}
/**
* Gets counter of number of processed views.
*
* @return counter of number of processed views.
*/
public int getViewCount() {
return viewCount;
}
/**
* Gets estimated fundamental matrix for current view.
* This fundamental matrix relates current view with the previously processed one.
*
* @return current estimated fundamental matrix.
*/
public EstimatedFundamentalMatrix getCurrentEstimatedFundamentalMatrix() {
return currentEstimatedFundamentalMatrix;
}
/**
* Gets estimated euclidean camera for current view (i.e. with actual scale).
*
* @return current estimated euclidean camera.
*/
public EstimatedCamera getCurrentEuclideanEstimatedCamera() {
return currentEuclideanEstimatedCamera;
}
/**
* Gets estimated Euclidean camera for previous view (i.e. with actual scale).
*
* @return previous estimated euclidean camera.
*/
public EstimatedCamera getPreviousEuclideanEstimatedCamera() {
return previousEuclideanEstimatedCamera;
}
/**
* Gets Euclidean reconstructed 3D points (i.e. with actual scale) for current
* pair of views.
*
* @return active euclidean reconstructed 3D points.
*/
public List<ReconstructedPoint3D> getEuclideanReconstructedPoints() {
return euclideanReconstructedPoints;
}
/**
* Gets current estimated scale. This will typically converge to a constant value as more views are
* processed.
* The smaller the variance of estimated scale, the more accurate the scale will be.
*
* @return current estimated scale.
*/
public double getCurrentScale() {
return currentScale;
}
/**
* Gets samples on previous view.
*
* @return samples on previous view.
*/
public List<Sample2D> getPreviousViewSamples() {
return previousViewSamples;
}
/**
* Gets samples on current view.
*
* @return samples on current view.
*/
public List<Sample2D> getCurrentViewSamples() {
return currentViewSamples;
}
/**
* Process one view-pair of all the available data during the reconstruction.
* This method can be called multiple times instead of {@link #start()} to build the
* reconstruction step by step, one view pair at a time.
* This method is useful when data is gathered on real time from a camera and the
* number of views is unknown.
*
* @return true if more views can be processed, false when reconstruction has finished.
*/
public boolean processOneViewPair() {
if (viewCount == 0 && !running) {
reset();
running = true;
//noinspection unchecked
listener.onStart((R) this);
}
//noinspection unchecked
if (!listener.hasMoreViewsAvailable((R) this)) {
//noinspection unchecked
listener.onFinish((R) this);
running = false;
finished = true;
return false;
}
previousViewSamples = new ArrayList<>();
currentViewSamples = new ArrayList<>();
//noinspection unchecked
listener.onRequestSamplesForCurrentViewPair((R) this, viewCount, viewCount + 1,
previousViewSamples, currentViewSamples);
final boolean processed;
currentEstimatedFundamentalMatrix = null;
if (isFirstViewPair()) {
// for first view we simply keep samples (if enough are provided)
processed = processFirstViewPair();
} else {
processed = processAdditionalViewPair();
}
if (processed) {
viewCount += 2;
}
if (cancelled) {
//noinspection unchecked
listener.onCancel((R) this);
}
return !finished;
}
/**
* Indicates whether current view pair is the first one.
*
* @return true if current view pair is the first one, false otherwise.
*/
public boolean isFirstViewPair() {
return viewCount == 0;
}
/**
* Indicates whether current view pair is an additional one.
*
* @return true if current view pair is an additional one, false otherwise.
*/
public boolean isAdditionalViewPair() {
return !isFirstViewPair();
}
/**
* Starts reconstruction of all available data to reconstruct the whole scene.
* If reconstruction has already started and is running, calling this method
* has no effect.
* This method is useful when all data is available before starting the reconstruction.
*/
public void start() {
if (running) {
// already started
return;
}
while (processOneViewPair()) {
if (cancelled) {
break;
}
}
}
/**
* Cancels reconstruction.
* If reconstruction has already been cancelled, calling this method has no effect.
*/
public void cancel() {
if (cancelled) {
// already cancelled
return;
}
cancelled = true;
}
/**
* Resets this instance so that a reconstruction can be started from the beginning without cancelling
* current one.
*/
public void reset() {
if (previousViewSamples != null) {
previousViewSamples.clear();
}
if (currentViewSamples != null) {
currentViewSamples.clear();
}
matches.clear();
cancelled = failed = false;
viewCount = 0;
running = false;
currentEstimatedFundamentalMatrix = null;
currentMetricEstimatedCamera = previousMetricEstimatedCamera = null;
metricReconstructedPoints = null;
currentScale = DEFAULT_SCALE;
currentEuclideanEstimatedCamera = previousEuclideanEstimatedCamera = null;
euclideanReconstructedPoints = null;
previousViewId = 0;
currentViewId = 0;
finished = false;
}
/**
* Gets estimated metric camera for current view (i.e. up to scale).
*
* @return current estimated metric camera.
*/
protected EstimatedCamera getCurrentMetricEstimatedCamera() {
return currentMetricEstimatedCamera;
}
/**
* Gets estimated camera for previous view (i.e. up to scale).
*
* @return previous estimated metric camera.
*/
protected EstimatedCamera getPreviousMetricEstimatedCamera() {
return previousMetricEstimatedCamera;
}
/**
* Gets metric reconstructed 3D points (i.e. up to scale) for current pair of views.
*
* @return active metric reconstructed 3D points.
*/
protected List<ReconstructedPoint3D> getMetricReconstructedPoints() {
return metricReconstructedPoints;
}
/**
* Transforms cameras on current pair of views so that they are referred to
* last kept location and rotation and upgrades cameras from metric stratum to
* Euclidean stratum.
*
* @param isInitialPairOfViews true if initial pair of views is being processed, false otherwise.
* @param hasAbsoluteOrientation true if absolute orientation is required, false otherwise.
* @return true if cameras were successfully transformed.
*/
protected boolean transformPairOfCamerasAndPoints(
final boolean isInitialPairOfViews, final boolean hasAbsoluteOrientation) {
if (isInitialPairOfViews) {
// initial pair does not need transformation
return true;
}
if (previousMetricEstimatedCamera == null || currentMetricEstimatedCamera == null) {
return false;
}
final var previousMetricCamera = previousMetricEstimatedCamera.getCamera();
final var currentMetricCamera = currentMetricEstimatedCamera.getCamera();
if (previousMetricCamera == null || currentMetricCamera == null) {
return false;
}
if (invMetricCameraRotation == null) {
invMetricCameraRotation = mLastMetricCameraRotation.inverseRotationAndReturnNew();
} else {
mLastMetricCameraRotation.inverseRotation(invMetricCameraRotation);
}
if (referenceMetricTransformation == null) {
referenceMetricTransformation = new EuclideanTransformation3D(invMetricCameraRotation);
} else {
referenceMetricTransformation.setRotation(invMetricCameraRotation);
}
referenceMetricTransformation.setTranslation(lastMetricCameraCenter);
try {
referenceMetricTransformation.transform(previousMetricCamera);
referenceMetricTransformation.transform(currentMetricCamera);
Point3D p;
for (final var metricReconstructedPoint : metricReconstructedPoints) {
p = metricReconstructedPoint.getPoint();
referenceMetricTransformation.transform(p, p);
}
return true;
} catch (final AlgebraException e) {
return false;
}
}
/**
* Processes data for the first view pair.
*
* @return true if view pair was successfully processed, false otherwise.
*/
private boolean processFirstViewPair() {
return processViewPair(true);
}
/**
* Processes data for an additional view pair.
*
* @return true if view pair was successfully processed, false otherwise.
*/
private boolean processAdditionalViewPair() {
return processViewPair(false);
}
/**
* Processed data for a view pair.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if view pair was successfully processed, false otherwise.
*/
private boolean processViewPair(final boolean isInitialPairOfViews) {
// for second view, check that we have enough samples
if (hasEnoughSamples(currentViewSamples)) {
// find matches
matches.clear();
var viewId1 = viewCount;
var viewId2 = viewCount + 1;
//noinspection unchecked
listener.onRequestMatches((R) this, viewId1, viewId2, previousViewSamples, currentViewSamples, matches);
if (hasEnoughMatches(matches)) {
// if enough matches are retrieved, attempt to compute
// fundamental matrix
if ((configuration.isGeneralSceneAllowed() && estimateFundamentalMatrix(matches, viewId1, viewId2))
|| (configuration.isPlanarSceneAllowed()
&& estimatePlanarFundamentalMatrix(matches, viewId1, viewId2))) {
// fundamental matrix could be estimated
// noinspection unchecked
listener.onSamplesAccepted((R) this, viewId1, viewId2, previousViewSamples, currentViewSamples);
previousViewId = viewId1;
currentViewId = viewId2;
//noinspection unchecked
listener.onFundamentalMatrixEstimated((R) this, viewId1, viewId2,
currentEstimatedFundamentalMatrix);
if (estimatePairOfCamerasAndPoints(isInitialPairOfViews)) {
//noinspection unchecked
listener.onEuclideanCameraPairEstimated((R) this, previousViewId, currentViewId, currentScale,
previousEuclideanEstimatedCamera, currentEuclideanEstimatedCamera);
//noinspection unchecked
listener.onEuclideanReconstructedPointsEstimated((R) this, previousViewId, currentViewId,
currentScale, euclideanReconstructedPoints);
return true;
} else {
// pair of cameras estimation failed
failed = true;
//noinspection unchecked
listener.onFail((R) this);
return false;
}
} else {
// estimation of fundamental matrix failed
//noinspection unchecked
listener.onSamplesRejected((R) this, previousViewId, currentViewId, previousViewSamples,
currentViewSamples);
return false;
}
}
}
//noinspection unchecked
listener.onSamplesRejected((R) this, previousViewId, currentViewId, previousViewSamples, currentViewSamples);
return false;
}
/**
* Indicates whether implementations of a re-constructor uses absolute orientation or
* not.
*
* @return true if absolute orientation is used, false, otherwise.
*/
protected abstract boolean hasAbsoluteOrientation();
/**
* Indicates whether there are enough samples to estimate a fundamental
* matrix.
*
* @param samples samples to check.
* @return true if there are enough samples, false otherwise.
*/
private boolean hasEnoughSamples(final List<Sample2D> samples) {
return hasEnoughSamplesOrMatches(samples != null ? samples.size() : 0);
}
/**
* Indicates whether there are enough matches to estimate a fundamental
* matrix.
*
* @param matches matches to check.
* @return true if there are enough matches, false otherwise.
*/
private boolean hasEnoughMatches(final List<MatchedSamples> matches) {
return hasEnoughSamplesOrMatches(matches != null ? matches.size() : 0);
}
/**
* Indicates whether there are enough matches or samples to estimate a
* fundamental matrix.
*
* @param count number of matches or samples.
* @return true if there are enough matches or samples, false otherwise.
*/
private boolean hasEnoughSamplesOrMatches(final int count) {
if (configuration.isGeneralSceneAllowed()) {
if (configuration.getNonRobustFundamentalMatrixEstimatorMethod()
== FundamentalMatrixEstimatorMethod.EIGHT_POINTS_ALGORITHM) {
return count >= EightPointsFundamentalMatrixEstimator.MIN_REQUIRED_POINTS;
} else if (configuration.getNonRobustFundamentalMatrixEstimatorMethod()
== FundamentalMatrixEstimatorMethod.SEVEN_POINTS_ALGORITHM) {
return count >= SevenPointsFundamentalMatrixEstimator.MIN_REQUIRED_POINTS;
}
} else if (configuration.isPlanarSceneAllowed()) {
return count >= ProjectiveTransformation2DRobustEstimator.MINIMUM_SIZE;
}
return false;
}
/**
* Estimates fundamental matrix for provided matches, when 3D points lay in
* a general non-degenerate 3D configuration.
*
* @param matches pairs of matches to find fundamental matrix.
* @param viewId1 id of first view.
* @param viewId2 id of second view.
* @return true if estimation succeeded, false otherwise.
*/
private boolean estimateFundamentalMatrix(final List<MatchedSamples> matches, final int viewId1,
final int viewId2) {
if (matches == null) {
return false;
}
final var count = matches.size();
final var leftSamples = new ArrayList<Sample2D>(count);
final var rightSamples = new ArrayList<Sample2D>(count);
final var leftPoints = new ArrayList<Point2D>(count);
final var rightPoints = new ArrayList<Point2D>(count);
final var qualityScores = new double[count];
double principalPointX;
double principalPointY;
if (configuration.getPairedCamerasEstimatorMethod() == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC
|| configuration.getPairedCamerasEstimatorMethod()
== InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC_AND_ESSENTIAL_MATRIX) {
principalPointX = configuration.getPrincipalPointX();
principalPointY = configuration.getPrincipalPointY();
} else {
principalPointX = principalPointY = 0.0;
}
var i = 0;
for (final var match : matches) {
final var samples = match.getSamples();
if (samples.length != MIN_NUMBER_OF_VIEWS) {
return false;
}
leftSamples.add(samples[0]);
rightSamples.add(samples[1]);
final var leftPoint = Point2D.create();
leftPoint.setInhomogeneousCoordinates(
samples[0].getPoint().getInhomX() - principalPointX,
samples[0].getPoint().getInhomY() - principalPointY);
leftPoints.add(leftPoint);
final var rightPoint = Point2D.create();
rightPoint.setInhomogeneousCoordinates(
samples[1].getPoint().getInhomX() - principalPointX,
samples[1].getPoint().getInhomY() - principalPointY);
rightPoints.add(rightPoint);
qualityScores[i] = match.getQualityScore();
i++;
}
try {
final var estimator = FundamentalMatrixRobustEstimator.create(leftPoints, rightPoints, qualityScores,
configuration.getRobustFundamentalMatrixEstimatorMethod());
estimator.setNonRobustFundamentalMatrixEstimatorMethod(
configuration.getNonRobustFundamentalMatrixEstimatorMethod());
estimator.setResultRefined(configuration.isFundamentalMatrixRefined());
estimator.setCovarianceKept(configuration.isFundamentalMatrixCovarianceKept());
estimator.setConfidence(configuration.getFundamentalMatrixConfidence());
estimator.setMaxIterations(configuration.getFundamentalMatrixMaxIterations());
switch (configuration.getRobustFundamentalMatrixEstimatorMethod()) {
case LMEDS:
((LMedSFundamentalMatrixRobustEstimator) estimator).setStopThreshold(
configuration.getFundamentalMatrixThreshold());
break;
case MSAC:
((MSACFundamentalMatrixRobustEstimator) estimator).setThreshold(
configuration.getFundamentalMatrixThreshold());
break;
case PROMEDS:
((PROMedSFundamentalMatrixRobustEstimator) estimator).setStopThreshold(
configuration.getFundamentalMatrixThreshold());
break;
case PROSAC:
final PROSACFundamentalMatrixRobustEstimator prosacEstimator =
(PROSACFundamentalMatrixRobustEstimator) estimator;
prosacEstimator.setThreshold(configuration.getFundamentalMatrixThreshold());
prosacEstimator.setComputeAndKeepInliersEnabled(
configuration.getFundamentalMatrixComputeAndKeepInliers());
prosacEstimator.setComputeAndKeepResidualsEnabled(
configuration.getFundamentalMatrixComputeAndKeepResiduals());
break;
case RANSAC:
final RANSACFundamentalMatrixRobustEstimator ransacEstimator =
(RANSACFundamentalMatrixRobustEstimator) estimator;
ransacEstimator.setThreshold(configuration.getFundamentalMatrixThreshold());
ransacEstimator.setComputeAndKeepInliersEnabled(
configuration.getFundamentalMatrixComputeAndKeepInliers());
ransacEstimator.setComputeAndKeepResidualsEnabled(
configuration.getFundamentalMatrixComputeAndKeepResiduals());
break;
default:
break;
}
final var fundamentalMatrix = estimator.estimate();
currentEstimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
currentEstimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
currentEstimatedFundamentalMatrix.setViewId1(viewId1);
currentEstimatedFundamentalMatrix.setViewId2(viewId2);
currentEstimatedFundamentalMatrix.setCovariance(estimator.getCovariance());
// determine quality score and inliers
final var inliersData = estimator.getInliersData();
if (inliersData != null) {
final var numInliers = inliersData.getNumInliers();
final var inliers = inliersData.getInliers();
final var length = inliers.length();
var fundamentalMatrixQualityScore = 0.0;
for (i = 0; i < length; i++) {
if (inliers.get(i)) {
// inlier
fundamentalMatrixQualityScore += qualityScores[i] / numInliers;
}
}
currentEstimatedFundamentalMatrix.setQualityScore(fundamentalMatrixQualityScore);
currentEstimatedFundamentalMatrix.setInliers(inliers);
}
// store left/right samples
currentEstimatedFundamentalMatrix.setLeftSamples(leftSamples);
currentEstimatedFundamentalMatrix.setRightSamples(rightSamples);
return true;
} catch (final Exception e) {
return false;
}
}
/**
* Estimates fundamental matrix for provided matches, when 3D points lay in
* a planar 3D scene.
*
* @param matches pairs of matches to find fundamental matrix.
* @param viewId1 id of first view.
* @param viewId2 id of second view.
* @return true if estimation succeeded, false otherwise.
*/
private boolean estimatePlanarFundamentalMatrix(final List<MatchedSamples> matches, final int viewId1,
final int viewId2) {
if (matches == null) {
return false;
}
final var count = matches.size();
final var leftSamples = new ArrayList<Sample2D>();
final var rightSamples = new ArrayList<Sample2D>();
final var leftPoints = new ArrayList<Point2D>();
final var rightPoints = new ArrayList<Point2D>();
final var qualityScores = new double[count];
final double principalPointX;
final double principalPointY;
if (configuration.getPairedCamerasEstimatorMethod() == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC
|| configuration.getPairedCamerasEstimatorMethod()
== InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC_AND_ESSENTIAL_MATRIX) {
principalPointX = configuration.getPrincipalPointX();
principalPointY = configuration.getPrincipalPointY();
} else {
principalPointX = principalPointY = 0.0;
}
var i = 0;
for (final var match : matches) {
final var samples = match.getSamples();
if (samples.length != MIN_NUMBER_OF_VIEWS) {
return false;
}
leftSamples.add(samples[0]);
rightSamples.add(samples[1]);
final var leftPoint = Point2D.create();
leftPoint.setInhomogeneousCoordinates(
samples[0].getPoint().getInhomX() - principalPointX,
samples[0].getPoint().getInhomY() - principalPointY);
leftPoints.add(leftPoint);
final var rightPoint = Point2D.create();
rightPoint.setInhomogeneousCoordinates(
samples[1].getPoint().getInhomX() - principalPointX,
samples[1].getPoint().getInhomY() - principalPointY);
rightPoints.add(rightPoint);
qualityScores[i] = match.getQualityScore();
i++;
}
try {
final var homographyEstimator = PointCorrespondenceProjectiveTransformation2DRobustEstimator.create(
configuration.getRobustPlanarHomographyEstimatorMethod());
homographyEstimator.setResultRefined(configuration.isPlanarHomographyRefined());
homographyEstimator.setCovarianceKept(configuration.isPlanarHomographyCovarianceKept());
homographyEstimator.setConfidence(configuration.getPlanarHomographyConfidence());
homographyEstimator.setMaxIterations(configuration.getPlanarHomographyMaxIterations());
switch (configuration.getRobustPlanarHomographyEstimatorMethod()) {
case LMEDS:
((LMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
.setStopThreshold(configuration.getPlanarHomographyThreshold());
break;
case MSAC:
((MSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
.setThreshold(configuration.getPlanarHomographyThreshold());
break;
case PROMEDS:
((PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
.setStopThreshold(configuration.getPlanarHomographyThreshold());
break;
case PROSAC:
PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator prosacHomographyEstimator =
(PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator;
prosacHomographyEstimator.setThreshold(configuration.getPlanarHomographyThreshold());
prosacHomographyEstimator.setComputeAndKeepInliersEnabled(
configuration.getPlanarHomographyComputeAndKeepInliers());
prosacHomographyEstimator.setComputeAndKeepResidualsEnabled(
configuration.getPlanarHomographyComputeAndKeepResiduals());
break;
case RANSAC:
RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator ransacHomographyEstimator =
(RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator;
ransacHomographyEstimator.setThreshold(configuration.getPlanarHomographyThreshold());
ransacHomographyEstimator.setComputeAndKeepInliersEnabled(
configuration.getPlanarHomographyComputeAndKeepInliers());
ransacHomographyEstimator.setComputeAndKeepResidualsEnabled(
configuration.getPlanarHomographyComputeAndKeepResiduals());
break;
default:
break;
}
final PlanarBestFundamentalMatrixEstimatorAndReconstructor fundamentalMatrixEstimator =
new PlanarBestFundamentalMatrixEstimatorAndReconstructor();
fundamentalMatrixEstimator.setHomographyEstimator(homographyEstimator);
fundamentalMatrixEstimator.setLeftAndRightPoints(leftPoints, rightPoints);
fundamentalMatrixEstimator.setQualityScores(qualityScores);
PinholeCameraIntrinsicParameters intrinsic1 = null;
PinholeCameraIntrinsicParameters intrinsic2 = null;
if (configuration.areIntrinsicParametersKnown()) {
//noinspection unchecked
intrinsic1 = listener.onIntrinsicParametersRequested((R) this, viewId1);
//noinspection unchecked
intrinsic2 = listener.onIntrinsicParametersRequested((R) this, viewId2);
}
if (intrinsic1 == null && intrinsic2 == null) {
// estimate homography
final var homography = homographyEstimator.estimate();
// estimate intrinsic parameters using the Image of Absolute
// Conic (IAC)
final var homographies = new ArrayList<Transformation2D>();
homographies.add(homography);
final var iacEstimator = new LMSEImageOfAbsoluteConicEstimator(homographies);
final var iac = iacEstimator.estimate();
intrinsic1 = intrinsic2 = iac.getIntrinsicParameters();
} else if (intrinsic1 == null) { // && intrinsic2 != null
intrinsic1 = intrinsic2;
} else if (intrinsic2 == null) { // && intrinsic1 != null
intrinsic2 = intrinsic1;
}
fundamentalMatrixEstimator.setLeftIntrinsics(intrinsic1);
fundamentalMatrixEstimator.setRightIntrinsics(intrinsic2);
fundamentalMatrixEstimator.estimateAndReconstruct();
final var fundamentalMatrix = fundamentalMatrixEstimator.getFundamentalMatrix();
currentEstimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
currentEstimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
currentEstimatedFundamentalMatrix.setViewId1(viewId1);
currentEstimatedFundamentalMatrix.setViewId2(viewId2);
// determine quality score and inliers
final var inliersData = homographyEstimator.getInliersData();
if (inliersData != null) {
final var numInliers = inliersData.getNumInliers();
final var inliers = inliersData.getInliers();
final var length = inliers.length();
var fundamentalMatrixQualityScore = 0.0;
for (i = 0; i < length; i++) {
if (inliers.get(i)) {
// inlier
fundamentalMatrixQualityScore += qualityScores[i] / numInliers;
}
}
currentEstimatedFundamentalMatrix.setQualityScore(
fundamentalMatrixQualityScore);
currentEstimatedFundamentalMatrix.setInliers(inliers);
}
// store left/right samples
currentEstimatedFundamentalMatrix.setLeftSamples(leftSamples);
currentEstimatedFundamentalMatrix.setRightSamples(rightSamples);
return true;
} catch (final Exception e) {
return false;
}
}
/**
* Estimates a pair of cameras and reconstructed points.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimatePairOfCamerasAndPoints(final boolean isInitialPairOfViews) {
return switch (configuration.getPairedCamerasEstimatorMethod()) {
case ESSENTIAL_MATRIX -> estimateInitialCamerasAndPointsEssential(isInitialPairOfViews);
case DUAL_IMAGE_OF_ABSOLUTE_CONIC -> estimateInitialCamerasAndPointsDIAC(isInitialPairOfViews);
case DUAL_ABSOLUTE_QUADRIC -> estimateInitialCamerasAndPointsDAQ(isInitialPairOfViews);
default -> estimateInitialCamerasAndPointsDAQAndEssential(isInitialPairOfViews);
};
}
/**
* Estimates initial cameras and reconstructed points using the Dual
* Absolute Quadric to estimate intrinsic parameters and then use those
* intrinsic parameters with the essential matrix.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimateInitialCamerasAndPointsDAQAndEssential(final boolean isInitialPairOfViews) {
// for non-initial view, keep last center and rotation
if (!isInitialPairOfViews && keepLastCenterAndRotation()) {
return false;
}
try {
final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
final var estimator = new DualAbsoluteQuadricInitialCamerasEstimator(fundamentalMatrix);
estimator.setAspectRatio(configuration.getPairedCamerasAspectRatio());
estimator.estimate();
final var camera1 = estimator.getEstimatedLeftCamera();
final var camera2 = estimator.getEstimatedRightCamera();
camera1.decompose();
camera2.decompose();
final var intrinsicZeroPrincipalPoint1 = camera1.getIntrinsicParameters();
final var intrinsicZeroPrincipalPoint2 = camera2.getIntrinsicParameters();
final var principalPointX = configuration.getPrincipalPointX();
final var principalPointY = configuration.getPrincipalPointY();
final var intrinsic1 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint1);
intrinsic1.setHorizontalPrincipalPoint(intrinsic1.getHorizontalPrincipalPoint() + principalPointX);
intrinsic1.setVerticalPrincipalPoint(intrinsic1.getVerticalPrincipalPoint() + principalPointY);
final var intrinsic2 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint2);
intrinsic2.setHorizontalPrincipalPoint(intrinsic2.getHorizontalPrincipalPoint() + principalPointX);
intrinsic2.setVerticalPrincipalPoint(intrinsic2.getVerticalPrincipalPoint() + principalPointY);
// fix fundamental matrix to account for principal point different
// from zero
fixFundamentalMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1, intrinsicZeroPrincipalPoint2,
intrinsic1, intrinsic2);
return estimateInitialCamerasAndPointsEssential(intrinsic1, intrinsic2)
&& transformPairOfCamerasAndPoints(isInitialPairOfViews, hasAbsoluteOrientation());
} catch (final Exception e) {
return false;
}
}
/**
* Estimates initial cameras and reconstructed points using the Dual
* Absolute Quadric.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimateInitialCamerasAndPointsDAQ(final boolean isInitialPairOfViews) {
// for non-initial view, keep last center and rotation
if (!isInitialPairOfViews && keepLastCenterAndRotation()) {
return false;
}
try {
final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
fundamentalMatrix.normalize();
final var estimator = new DualAbsoluteQuadricInitialCamerasEstimator(fundamentalMatrix);
estimator.setAspectRatio(configuration.getPairedCamerasAspectRatio());
estimator.estimate();
final var camera1 = estimator.getEstimatedLeftCamera();
final var camera2 = estimator.getEstimatedRightCamera();
camera1.decompose();
camera2.decompose();
final var intrinsicZeroPrincipalPoint1 = camera1.getIntrinsicParameters();
final var intrinsicZeroPrincipalPoint2 = camera2.getIntrinsicParameters();
final var principalPointX = configuration.getPrincipalPointX();
final var principalPointY = configuration.getPrincipalPointY();
final var intrinsic1 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint1);
intrinsic1.setHorizontalPrincipalPoint(intrinsic1.getHorizontalPrincipalPoint() + principalPointX);
intrinsic1.setVerticalPrincipalPoint(intrinsic1.getVerticalPrincipalPoint() + principalPointY);
camera1.setIntrinsicParameters(intrinsic1);
final var intrinsic2 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint2);
intrinsic2.setHorizontalPrincipalPoint(intrinsic2.getHorizontalPrincipalPoint() + principalPointX);
intrinsic2.setVerticalPrincipalPoint(intrinsic2.getVerticalPrincipalPoint() + principalPointY);
camera2.setIntrinsicParameters(intrinsic2);
previousMetricEstimatedCamera = new EstimatedCamera();
previousMetricEstimatedCamera.setCamera(camera1);
currentMetricEstimatedCamera = new EstimatedCamera();
currentMetricEstimatedCamera.setCamera(camera2);
// fix fundamental matrix to account for principal point different
// from zero
fixFundamentalMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1, intrinsicZeroPrincipalPoint2,
intrinsic1, intrinsic2);
// triangulate points
Corrector corrector = null;
if (configuration.getPairedCamerasCorrectorType() != null) {
corrector = Corrector.create(fundamentalMatrix, configuration.getPairedCamerasCorrectorType());
}
// use all points used for fundamental matrix estimation
final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
final var points1 = new ArrayList<Point2D>();
final var points2 = new ArrayList<Point2D>();
final var length = samples1.size();
for (var i = 0; i < length; i++) {
final var sample1 = samples1.get(i);
final var sample2 = samples2.get(i);
final var point1 = sample1.getPoint();
final var point2 = sample2.getPoint();
points1.add(point1);
points2.add(point2);
}
// correct points if needed
final List<Point2D> correctedPoints1;
final List<Point2D> correctedPoints2;
if (corrector != null) {
corrector.setLeftAndRightPoints(points1, points2);
corrector.correct();
correctedPoints1 = corrector.getLeftCorrectedPoints();
correctedPoints2 = corrector.getRightCorrectedPoints();
} else {
correctedPoints1 = points1;
correctedPoints2 = points2;
}
// triangulate points
final SinglePoint3DTriangulator triangulator;
if (configuration.getDaqUseHomogeneousPointTriangulator()) {
triangulator = SinglePoint3DTriangulator.create(Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR);
} else {
triangulator = SinglePoint3DTriangulator.create(
Point3DTriangulatorType.LMSE_INHOMOGENEOUS_TRIANGULATOR);
}
final var cameras = new ArrayList<PinholeCamera>();
cameras.add(camera1);
cameras.add(camera2);
metricReconstructedPoints = new ArrayList<>();
final var points = new ArrayList<Point2D>();
final var numPoints = correctedPoints1.size();
Point3D triangulatedPoint;
ReconstructedPoint3D reconstructedPoint;
for (var i = 0; i < numPoints; i++) {
points.clear();
points.add(correctedPoints1.get(i));
points.add(correctedPoints2.get(i));
triangulator.setPointsAndCameras(points, cameras);
triangulatedPoint = triangulator.triangulate();
reconstructedPoint = new ReconstructedPoint3D();
reconstructedPoint.setPoint(triangulatedPoint);
// only points reconstructed in front of both cameras are
// considered valid
final var front1 = camera1.isPointInFrontOfCamera(triangulatedPoint);
final var front2 = camera2.isPointInFrontOfCamera(triangulatedPoint);
reconstructedPoint.setInlier(front1 && front2);
metricReconstructedPoints.add(reconstructedPoint);
}
return transformPairOfCamerasAndPoints(isInitialPairOfViews, hasAbsoluteOrientation());
} catch (final Exception e) {
return false;
}
}
/**
* Estimates initial cameras and reconstructed points using Dual Image of
* Absolute Conic.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimateInitialCamerasAndPointsDIAC(final boolean isInitialPairOfViews) {
// for non-initial view, keep last center and rotation
if (!isInitialPairOfViews && keepLastCenterAndRotation()) {
return false;
}
final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
// use inlier points used for fundamental matrix estimation
final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
final var points1 = new ArrayList<Point2D>();
final var points2 = new ArrayList<Point2D>();
final var length = samples1.size();
for (var i = 0; i < length; i++) {
final var sample1 = samples1.get(i);
final var sample2 = samples2.get(i);
final var point1 = sample1.getPoint();
final var point2 = sample2.getPoint();
points1.add(point1);
points2.add(point2);
}
try {
final var estimator = new DualImageOfAbsoluteConicInitialCamerasEstimator(fundamentalMatrix, points1,
points2);
estimator.setPrincipalPoint(configuration.getPrincipalPointX(), configuration.getPrincipalPointY());
estimator.setAspectRatio(configuration.getPairedCamerasAspectRatio());
estimator.setCorrectorType(configuration.getPairedCamerasCorrectorType());
estimator.setPointsTriangulated(true);
estimator.setValidTriangulatedPointsMarked(configuration.getPairedCamerasMarkValidTriangulatedPoints());
estimator.estimate();
// store cameras
final var camera1 = estimator.getEstimatedLeftCamera();
final var camera2 = estimator.getEstimatedRightCamera();
previousMetricEstimatedCamera = new EstimatedCamera();
previousMetricEstimatedCamera.setCamera(camera1);
currentMetricEstimatedCamera = new EstimatedCamera();
currentMetricEstimatedCamera.setCamera(camera2);
// store points
final var triangulatedPoints = estimator.getTriangulatedPoints();
final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
metricReconstructedPoints = new ArrayList<>();
final var size = triangulatedPoints.size();
for (var i = 0; i < size; i++) {
final var reconstructedPoint = new ReconstructedPoint3D();
reconstructedPoint.setPoint(triangulatedPoints.get(i));
reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
metricReconstructedPoints.add(reconstructedPoint);
}
return transformPairOfCamerasAndPoints(isInitialPairOfViews, hasAbsoluteOrientation());
} catch (final Exception e) {
return false;
}
}
/**
* Estimates initial cameras and reconstructed points using the essential
* matrix and provided intrinsic parameters that must have been set during
* offline calibration.
*
* @param isInitialPairOfViews true if initial pair of views is being processed,
* false otherwise.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimateInitialCamerasAndPointsEssential(final boolean isInitialPairOfViews) {
// for non-initial view, keep last center and rotation
if (!isInitialPairOfViews && keepLastCenterAndRotation()) {
return false;
}
PinholeCameraIntrinsicParameters intrinsic1 = null;
PinholeCameraIntrinsicParameters intrinsic2 = null;
if (configuration.areIntrinsicParametersKnown()) {
//noinspection unchecked
intrinsic1 = listener.onIntrinsicParametersRequested((R) this, previousViewId);
//noinspection unchecked
intrinsic2 = listener.onIntrinsicParametersRequested((R) this, currentViewId);
}
if (intrinsic1 != null && intrinsic2 != null) {
return estimateInitialCamerasAndPointsEssential(intrinsic1, intrinsic2)
&& transformPairOfCamerasAndPoints(isInitialPairOfViews, hasAbsoluteOrientation());
} else {
// missing intrinsic parameters
failed = true;
//noinspection unchecked
listener.onFail((R) this);
return false;
}
}
/**
* Estimates initial cameras and reconstructed points using the essential
* matrix and provided intrinsic parameters that must have been set during
* offline calibration.
*
* @param intrinsic1 intrinsic parameters of 1st camera.
* @param intrinsic2 intrinsic parameters of 2nd camera.
* @return true if cameras and points could be estimated, false if something
* failed.
*/
private boolean estimateInitialCamerasAndPointsEssential(
final PinholeCameraIntrinsicParameters intrinsic1, final PinholeCameraIntrinsicParameters intrinsic2) {
final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
// use all points used for fundamental matrix estimation
final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
final var points1 = new ArrayList<Point2D>();
final var points2 = new ArrayList<Point2D>();
final var length = samples1.size();
for (var i = 0; i < length; i++) {
final var sample1 = samples1.get(i);
final var sample2 = samples2.get(i);
final var point1 = sample1.getPoint();
final var point2 = sample2.getPoint();
points1.add(point1);
points2.add(point2);
}
try {
final var estimator = new EssentialMatrixInitialCamerasEstimator(fundamentalMatrix, intrinsic1, intrinsic2,
points1, points2);
estimator.setCorrectorType(configuration.getPairedCamerasCorrectorType());
estimator.setPointsTriangulated(true);
estimator.setValidTriangulatedPointsMarked(configuration.getPairedCamerasMarkValidTriangulatedPoints());
estimator.estimate();
// store cameras
final var camera1 = estimator.getEstimatedLeftCamera();
final var camera2 = estimator.getEstimatedRightCamera();
previousMetricEstimatedCamera = new EstimatedCamera();
previousMetricEstimatedCamera.setCamera(camera1);
currentMetricEstimatedCamera = new EstimatedCamera();
currentMetricEstimatedCamera.setCamera(camera2);
// store points
final var triangulatedPoints = estimator.getTriangulatedPoints();
final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
metricReconstructedPoints = new ArrayList<>();
final var size = triangulatedPoints.size();
for (var i = 0; i < size; i++) {
final var reconstructedPoint = new ReconstructedPoint3D();
reconstructedPoint.setPoint(triangulatedPoints.get(i));
reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
metricReconstructedPoints.add(reconstructedPoint);
}
return true;
} catch (final Exception e) {
return false;
}
}
/**
* Keeps center and rotation of last camera (current camera on previous view pair).
*
* @return false if camera and rotation were successfully kept, true otherwise.
*/
private boolean keepLastCenterAndRotation() {
// keep last metric center and rotation
if (currentMetricEstimatedCamera == null || currentEuclideanEstimatedCamera == null) {
return true;
}
final var metricCamera = currentMetricEstimatedCamera.getCamera();
if (metricCamera == null) {
return true;
}
try {
// decompose camera if needed
if (!metricCamera.isCameraCenterAvailable() || !metricCamera.isCameraRotationAvailable()) {
metricCamera.decompose();
}
lastMetricCameraCenter = metricCamera.getCameraCenter();
mLastMetricCameraRotation = metricCamera.getCameraRotation();
} catch (final GeometryException e) {
return true;
}
// keep last Euclidean center and rotation
final var euclideanCamera = currentEuclideanEstimatedCamera.getCamera();
if (euclideanCamera == null) {
return true;
}
try {
// decompose camera if needed
if (!euclideanCamera.isCameraCenterAvailable() || !euclideanCamera.isCameraRotationAvailable()) {
euclideanCamera.decompose();
}
lastEuclideanCameraCenter = euclideanCamera.getCameraCenter();
lastEuclideanCameraRotation = euclideanCamera.getCameraRotation();
return false;
} catch (final GeometryException e) {
return true;
}
}
/**
* Fixes fundamental matrix to account for principal point different from
* zero when using DAQ estimation.
*
* @param fundamentalMatrix fundamental matrix to be fixed.
* @param intrinsicZeroPrincipalPoint1 intrinsic parameters of camera 1
* assuming zero principal point.
* @param intrinsicZeroPrincipalPoint2 intrinsic parameters of camera 2
* assuming zero principal point.
* @param intrinsicPrincipalPoint1 intrinsic parameters of camera 1 using
* proper principal point.
* @param intrinsicPrincipalPoint2 intrinsic parameters of camera 2 using
* proper principal point.
* @throws EpipolarException if something fails.
* @throws NotReadyException never happens.
*/
private void fixFundamentalMatrix(final FundamentalMatrix fundamentalMatrix,
final PinholeCameraIntrinsicParameters intrinsicZeroPrincipalPoint1,
final PinholeCameraIntrinsicParameters intrinsicZeroPrincipalPoint2,
final PinholeCameraIntrinsicParameters intrinsicPrincipalPoint1,
final PinholeCameraIntrinsicParameters intrinsicPrincipalPoint2)
throws EpipolarException, NotReadyException {
// first compute essential matrix as E = K2a'F*K1a
final var essential = new EssentialMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1,
intrinsicZeroPrincipalPoint2);
final var fixedFundamentalMatrix = essential.toFundamentalMatrix(intrinsicPrincipalPoint1,
intrinsicPrincipalPoint2);
fixedFundamentalMatrix.normalize();
currentEstimatedFundamentalMatrix.setFundamentalMatrix(fixedFundamentalMatrix);
currentEstimatedFundamentalMatrix.setCovariance(null);
}
}