DualAbsoluteQuadricRobustEstimator.java
/*
* Copyright (C) 2016 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.calibration.estimators;
import com.irurueta.ar.calibration.DualAbsoluteQuadric;
import com.irurueta.geometry.PinholeCamera;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;
import com.irurueta.numerical.robust.RobustEstimatorException;
import com.irurueta.numerical.robust.RobustEstimatorMethod;
import java.util.List;
/**
* This is an abstract class for algorithms to robustly find the best
* DualAbsoluteQuadric (DAQ) for provided collection of cameras.
* Implementations of this class should be able to detect and discard outliers
* in order to find the best solution.
*/
@SuppressWarnings("DuplicatedCode")
public abstract class DualAbsoluteQuadricRobustEstimator {
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Default amount of progress variation before notifying a change in
* estimation progress. By default, this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Cameras to estimate dual absolute quadric (DAQ).
*/
protected List<PinholeCamera> cameras;
/**
* Internal non-robust estimator of DAQ.
*/
protected final LMSEDualAbsoluteQuadricEstimator daqEstimator;
/**
* Listener to be notified of events such as when estimation starts, ends or
* its progress significantly changes.
*/
protected DualAbsoluteQuadricRobustEstimatorListener listener;
/**
* Indicates if this estimator is locked because an estimation is being
* computed.
*/
protected boolean locked;
/**
* Amount of progress variation before notifying a progress change during
* estimation.
*/
protected float progressDelta;
/**
* Amount of confidence 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.
*/
protected double confidence;
/**
* Maximum allowed number of iterations. When the maximum number of
* iterations is exceeded, result will not be available, however an
* approximate result will be available for retrieval.
*/
protected int maxIterations;
/**
* Constructor.
*/
protected DualAbsoluteQuadricRobustEstimator() {
progressDelta = DEFAULT_PROGRESS_DELTA;
confidence = DEFAULT_CONFIDENCE;
maxIterations = DEFAULT_MAX_ITERATIONS;
daqEstimator = new LMSEDualAbsoluteQuadricEstimator();
}
/**
* Constructor.
*
* @param listener listener to be notified of events such as when
* estimation starts, ends or its progress significantly changes.
*/
protected DualAbsoluteQuadricRobustEstimator(final DualAbsoluteQuadricRobustEstimatorListener listener) {
this();
this.listener = listener;
}
/**
* Constructor.
*
* @param cameras list of cameras used to estimate the dual absolute
* quadric (DAQ), which can be used to obtain pinhole camera intrinsic
* parameters.
* @throws IllegalArgumentException if not enough cameras are provided
* for default settings. Hence, at least 2 cameras must be provided.
*/
protected DualAbsoluteQuadricRobustEstimator(final List<PinholeCamera> cameras) {
this();
internalSetCameras(cameras);
}
/**
* Constructor.
*
* @param cameras list of cameras used to estimate the dual absolute
* quadric (DAQ), which can be used to obtain pinhole camera intrinsic
* parameters.
* @param listener listener to be notified of events such as when
* estimation starts, ends or its progress significantly changes.
* @throws IllegalArgumentException if not enough cameras are provided
* for default settings. Hence, at least 2 cameras must be provided.
*/
protected DualAbsoluteQuadricRobustEstimator(
final List<PinholeCamera> cameras, final DualAbsoluteQuadricRobustEstimatorListener listener) {
this(listener);
internalSetCameras(cameras);
}
/**
* Returns boolean indicating whether camera skewness is assumed to be zero
* or not.
* Skewness determines whether LCD sensor cells are properly aligned or not,
* where zero indicates perfect alignment.
* Typically, skewness is a value equal or very close to zero.
*
* @return true if camera skewness is assumed to be zero, otherwise camera
* skewness is estimated.
*/
public boolean isZeroSkewness() {
return daqEstimator.isZeroSkewness();
}
/**
* Sets boolean indicating whether camera skewness is assumed to be zero or
* not.
* Skewness determines whether LCD sensor cells are properly aligned or not,
* where zero indicates perfect alignment.
* Typically, skewness is a value equal or very close to zero.
*
* @param zeroSkewness true if camera skewness is assumed to be zero,
* otherwise camera skewness is estimated.
* @throws LockedException if estimator is locked.
*/
public void setZeroSkewness(final boolean zeroSkewness) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setZeroSkewness(zeroSkewness);
}
/**
* Returns boolean indicating whether principal point is assumed to be at
* origin of coordinates or not.
* Typically principal point is located at image center (origin of
* coordinates), and usually matches the center of radial distortion if
* it is taken into account.
*
* @return true if principal point is assumed to be at origin of
* coordinates, false if principal point must be estimated
*/
public boolean isPrincipalPointAtOrigin() {
return daqEstimator.isPrincipalPointAtOrigin();
}
/**
* Sets boolean indicating whether principal point is assumed to be at
* origin of coordinates or not.
* Typically principal point is located at image center (origin of
* coordinates), and usually matches the center of radial distortion if it
* is taken into account.
*
* @param principalPointAtOrigin true if principal point is assumed to be at
* origin of coordinates, false if principal point must be estimated.
* @throws LockedException if estimator is locked.
*/
public void setPrincipalPointAtOrigin(final boolean principalPointAtOrigin) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setPrincipalPointAtOrigin(principalPointAtOrigin);
}
/**
* Returns boolean indicating whether aspect ratio of focal distances (i.e.
* vertical focal distance divided by horizontal focal distance) is known or
* not.
* Notice that focal distance aspect ratio is not related to image size
* aspect ratio. Typically, LCD sensor cells are square and hence aspect
* ratio of focal distances is known and equal to 1.
* This value is only taken into account if skewness is assumed to be zero,
* otherwise it is ignored.
*
* @return true if focal distance aspect ratio is known, false otherwise.
*/
public boolean isFocalDistanceAspectRatioKnown() {
return daqEstimator.isFocalDistanceAspectRatioKnown();
}
/**
* Sets value indicating whether aspect ratio of focal distances (i.e.
* vertical focal distance divided by horizontal focal distance) is known or
* not.
* Notice that focal distance aspect ratio is not related to image size
* aspect ratio. Typically, LCD sensor cells are square and hence aspect
* ratio of focal distances is known and equal to 1.
* This value is only taken into account if skewness is assumed to be zero,
* otherwise it is ignored.
*
* @param focalDistanceAspectRatioKnown true if focal distance aspect ratio
* is known, false otherwise.
* @throws LockedException if estimator is locked.
*/
public void setFocalDistanceAspectRatioKnown(final boolean focalDistanceAspectRatioKnown) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setFocalDistanceAspectRatioKnown(focalDistanceAspectRatioKnown);
}
/**
* Returns aspect ratio of focal distances (i.e. vertical focal distance
* divided by horizontal focal distance).
* This value is only taken into account if skewness is assumed to be zero
* and focal distance aspect ratio is marked as known, otherwise it is
* ignored.
* By default, this is 1.0, since it is taken into account that typically
* LCD sensor cells are square and hence aspect ratio focal distances is
* known and equal to 1.
* Notice that focal distance aspect ratio is not related to image size
* aspect ratio.
* Notice that a negative aspect ratio indicates that vertical axis is
* reversed. This can be useful in some situations where image vertical
* coordinates are reversed respect to the physical world (i.e. in computer
* graphics typically image vertical coordinates go downwards, while in
* physical world they go upwards).
*
* @return aspect ratio of focal distances.
*/
public double getFocalDistanceAspectRatio() {
return daqEstimator.getFocalDistanceAspectRatio();
}
/**
* Sets aspect ratio of focal distances (i.e. vertical focal distance
* divided by horizontal focal distance).
* This value is only taken into account if skewness is assumed to be zero
* and focal distance aspect ratio is marked as known, otherwise it is
* ignored.
* By default, this is 1.0, since it is taken into account that typically
* LCD sensor cells are square and hence aspect ratio focal distances is
* known and equal to 1.
* Notice that focal distance aspect ratio is not related to image size
* aspect ratio
* Notice that a negative aspect ratio indicates that vertical axis is
* reversed. This can be useful in some situations where image vertical
* coordinates are reversed respect to the physical world (i.e. in computer
* graphics typically image vertical coordinates go downwards, while in
* physical world they go upwards).
*
* @param focalDistanceAspectRatio aspect ratio of focal distances to be set.
* @throws LockedException if estimator is locked.
* @throws IllegalArgumentException if focal distance aspect ratio is too
* close to zero, as it might produce numerical instabilities.
*/
public void setFocalDistanceAspectRatio(final double focalDistanceAspectRatio) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setFocalDistanceAspectRatio(focalDistanceAspectRatio);
}
/**
* Indicates whether a singular DAQ is enforced or not.
* Dual Absolute Quadric is singular (has rank 3) in any projective space,
* however, due to noise in samples, estimated DAQ might not be fully
* singular.
*
* @return true when singular DAQ is enforced, false otherwise.
*/
public boolean isSingularityEnforced() {
return daqEstimator.isSingularityEnforced();
}
/**
* Specifies whether a singular DAQ is enforced or not.
* Dual Absolute Quadric is singular (has rank 3) in any projective space,
* however, due to noise in samples, estimated DAQ might not be fully
* singular.
*
* @param singularityEnforced true when singular DAQ is enforced, false
* otherwise.
* @throws LockedException if estimator is locked.
*/
public void setSingularityEnforced(final boolean singularityEnforced) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setSingularityEnforced(singularityEnforced);
}
/**
* Indicates whether enforced singularity will be validated by checking that
* determinant of estimated Dual Absolute Quadric (DAQ) is below a certain
* threshold.
*
* @return true if enforced singularity is validated, false otherwise.
*/
public boolean isEnforcedSingularityValidated() {
return daqEstimator.isEnforcedSingularityValidated();
}
/**
* Specifies whether enforced singularity will be validated by checking that
* determinant of estimated Dual Absolute Quadric (DAQ) is below a certain
* threshold.
*
* @param validateEnforcedSingularity true if enforced singularity is
* validated, false otherwise.
* @throws LockedException if estimator is locked.
*/
public void setEnforcedSingularityValidated(final boolean validateEnforcedSingularity) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setEnforcedSingularityValidated(validateEnforcedSingularity);
}
/**
* Returns threshold to determine whether estimated Dual Absolute Quadric
* (DAQ) has rank 3 or not when validation is enabled.
*
* @return threshold to determine whether estimated DAQ has rank 3 or not.
*/
public double getDeterminantThreshold() {
return daqEstimator.getDeterminantThreshold();
}
/**
* Sets threshold to determine whether estimated Dual Absolute Quadric (DAQ)
* has rank 3 or not when validation is enabled.
*
* @param determinantThreshold threshold to determine whether estimated DAQ
* has rank 3 or not.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if estimator is locked.
*/
public void setDeterminantThreshold(final double determinantThreshold) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
daqEstimator.setDeterminantThreshold(determinantThreshold);
}
/**
* Returns reference to listener to be notified of events such as when
* estimation starts, ends or its progress significantly changes.
*
* @return listener to be notified of events.
*/
public DualAbsoluteQuadricRobustEstimatorListener getListener() {
return listener;
}
/**
* Sets listener to be notified of events such as when estimation starts,
* ends or its progress significantly changes.
*
* @param listener listener to be notified of events.
* @throws LockedException if robust estimator is locked.
*/
public void setListener(final DualAbsoluteQuadricRobustEstimatorListener listener) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Indicates whether listener has been provided and is available for
* retrieval.
*
* @return true if available, false otherwise.
*/
public boolean isListenerAvailable() {
return listener != null;
}
/**
* Indicates whether this instance is locked.
*
* @return true if this estimator is busy estimating the Dual Absolute
* Quadric, false otherwise.
*/
public boolean isLocked() {
return locked;
}
/**
* Returns amount of progress variation before notifying a progress change
* during estimation.
*
* @return amount of progress variation before notifying a progress change
* during estimation.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change
* during estimation.
*
* @param progressDelta amount of progress variation before notifying a
* progress change during estimation.
* @throws IllegalArgumentException if progress delta is less than zero or
* greater than 1.
* @throws LockedException if this estimator is locked because an estimation
* is being computed.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence 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 getConfidence() {
return confidence;
}
/**
* Sets amount of confidence 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 this estimator is locked because an estimator
* is being computed.
*/
public void setConfidence(final double confidence) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number
* of iterations is achieved without converging to a result when calling
* estimate(), a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of
* iterations is exceeded, result will not be available, however an
* approximate result will be available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if this estimator is locked because an estimation
* is being computed.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Obtains the list of cameras used to estimate the Dual Absolute Quadric
* (DAQ).
*
* @return list of cameras to estimate the DAQ.
*/
public List<PinholeCamera> getCameras() {
return cameras;
}
/**
* Sets the list of cameras used to estimate the Dual Absolute Quadric
* (DAQ).
*
* @param cameras list of cameras used to estimate the DAQ.
* @throws IllegalArgumentException if list is null.
* @throws LockedException if estimator is locked.
*/
public final void setCameras(final List<PinholeCamera> cameras) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
internalSetCameras(cameras);
}
/**
* Returns minimum number of required cameras needed to estimate the
* Dual Absolute Quadric (DAQ).
* At least 8 equations are needed to solve the DAQ
* For each imposed constraint, one less equation is required.
* Depending on the number of constraints more or less cameras will be
* required.
* If zero skewness is enforced, a solution is available with 8 cameras.
* If zero skewness and focal distance aspect ratio is known, then a
* solution is available with 4 cameras.
* If principal point is located at origin, then a solution is available
* with 4 cameras.
* If zero skewness and principal point at origin are enforced, then a
* solution is available with 3 cameras
* If zero skewness is enforced, focal distance aspect ratio is known and
* principal point is at origin, then a solution is available with 2
* cameras.
* NOTE: minimum number of cameras considers only the cameras providing
* additional information. If a camera is equivalent to another one or does
* not provide additional information (such as a camera at the origin with
* no rotation), then more cameras will be needed.
*
* @return minimum number of required cameras needed to estimate the Dual
* Absolute Quadric (DAQ) or -1 if constraints configurations is not valid.
*/
public int getMinNumberOfRequiredCameras() {
return daqEstimator.getMinNumberOfRequiredCameras();
}
/**
* Indicates whether current constraints are enough to start the estimation.
* In order to obtain a linear solution for the DAQ estimation, we need at
* least the principal point at origin constraint.
*
* @return true if constraints are valid, false otherwise.
*/
public boolean areValidConstraints() {
return daqEstimator.areValidConstraints();
}
/**
* Returns value indicating whether required data has been provided so that
* DAQ estimation can start.
* If true, estimator is ready to compute the DAQ, otherwise more data needs
* to be provided.
*
* @return true if estimator is ready, false otherwise.
*/
public boolean isReady() {
return cameras != null && cameras.size() >= getMinNumberOfRequiredCameras() && areValidConstraints();
}
/**
* Returns quality scores corresponding to each camera.
* The larger the score value the better the quality of the camera.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behaviour.
*
* @return quality scores corresponding to each camera.
*/
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each camera.
* The larger the score value the better the quality of the camera.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each camera.
* @throws LockedException if robust estimator is locked because an
* estimation is already in progress.
* @throws IllegalArgumentException if provided quality scores length is
* smaller than minimum required number of cameras.
*/
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Estimates the Dual Absolute Quadric using provided cameras.
*
* @return estimated Dual Absolute Quadric (DAQ).
* @throws LockedException if robust estimator is locked.
* @throws NotReadyException if no valid input data has already been
* provided.
* @throws RobustEstimatorException if estimation fails for any reason
* (i.e. numerical instability, no solution available, etc).
*/
public abstract DualAbsoluteQuadric estimate() throws LockedException, NotReadyException, RobustEstimatorException;
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a dual absolute quadric robust estimator using provided method.
*
* @param method method of a robust estimator algorithm to estimate best
* DAQ.
* @return an instance of a dual absolute quadric robust estimator.
*/
public static DualAbsoluteQuadricRobustEstimator create(final RobustEstimatorMethod method) {
return switch (method) {
case MSAC -> new MSACDualAbsoluteQuadricRobustEstimator();
case RANSAC -> new RANSACDualAbsoluteQuadricRobustEstimator();
case PROSAC -> new PROSACDualAbsoluteQuadricRobustEstimator();
case PROMEDS -> new PROMedSDualAbsoluteQuadricRobustEstimator();
default -> new LMedSDualAbsoluteQuadricRobustEstimator();
};
}
/**
* Creates a dual absolute quadric robust estimator using provided
* cameras.
*
* @param cameras list of cameras.
* @param qualityScores quality scores corresponding to each camera.
* @param method method of a robust estimator algorithm to estimate best
* DAQ.
* @return an instance of a dual absolute quadric robust estimator.
* @throws IllegalArgumentException if provided list of cameras and quality
* scores don't have the same size or size is too short.
*/
public static DualAbsoluteQuadricRobustEstimator create(
final List<PinholeCamera> cameras, final double[] qualityScores, final RobustEstimatorMethod method) {
return switch (method) {
case MSAC -> new MSACDualAbsoluteQuadricRobustEstimator(cameras);
case RANSAC -> new RANSACDualAbsoluteQuadricRobustEstimator(cameras);
case PROSAC -> new PROSACDualAbsoluteQuadricRobustEstimator(cameras, qualityScores);
case PROMEDS -> new PROMedSDualAbsoluteQuadricRobustEstimator(cameras, qualityScores);
default -> new LMedSDualAbsoluteQuadricRobustEstimator(cameras);
};
}
/**
* Creates a dual absolute quadric robust estimator using provided
* cameras.
*
* @param cameras list of cameras.
* @param method method of a robust estimator algorithm to estimate
* best DAQ.
* @return an instance of a dual absolute quadric robust estimator.
* @throws IllegalArgumentException if provided list of cameras is too
* short.
*/
public static DualAbsoluteQuadricRobustEstimator create(
final List<PinholeCamera> cameras, final RobustEstimatorMethod method) {
return switch (method) {
case MSAC -> new MSACDualAbsoluteQuadricRobustEstimator(cameras);
case RANSAC -> new RANSACDualAbsoluteQuadricRobustEstimator(cameras);
case PROSAC -> new PROSACDualAbsoluteQuadricRobustEstimator(cameras);
case PROMEDS -> new PROMedSDualAbsoluteQuadricRobustEstimator(cameras);
default -> new LMedSDualAbsoluteQuadricRobustEstimator(cameras);
};
}
/**
* Creates a dual absolute quadric robust estimator using default method.
*
* @return an instance of a dual absolute quadric robust estimator.
*/
public static DualAbsoluteQuadricRobustEstimator create() {
return create(DEFAULT_ROBUST_METHOD);
}
/**
* Creates a dual absolute quadric robust estimator using provided
* cameras.
*
* @param cameras list of cameras.
* @param qualityScores quality scores corresponding to each camera.
* @return an instance of a dual absolute quadric robust estimator.
* @throws IllegalArgumentException if provided list of cameras and quality
* scores don't have the same size or size is too short.
*/
public static DualAbsoluteQuadricRobustEstimator create(
final List<PinholeCamera> cameras, final double[] qualityScores) {
return create(cameras, qualityScores, DEFAULT_ROBUST_METHOD);
}
/**
* Creates a dual absolute quadric robust estimator using provided
* cameras.
*
* @param cameras list of cameras.
* @return an instance of a dual absolute quadric robust estimator.
* @throws IllegalArgumentException if provided list of cameras is too
* short.
*/
public static DualAbsoluteQuadricRobustEstimator create(final List<PinholeCamera> cameras) {
return create(cameras, DEFAULT_ROBUST_METHOD);
}
/**
* Computes the residual between a dual absolute quadric (DAQ) and a pinhole
* camera.
*
* @param daq a dual absolute quadric (DAQ).
* @param camera a camera.
* @return residual.
*/
protected double residual(final DualAbsoluteQuadric daq, final PinholeCamera camera) {
daq.normalize();
camera.normalize();
final var cameraMatrix = camera.getInternalMatrix();
final var p11 = cameraMatrix.getElementAt(0, 0);
final var p21 = cameraMatrix.getElementAt(1, 0);
final var p31 = cameraMatrix.getElementAt(2, 0);
final var p12 = cameraMatrix.getElementAt(0, 1);
final var p22 = cameraMatrix.getElementAt(1, 1);
final var p32 = cameraMatrix.getElementAt(2, 1);
final var p13 = cameraMatrix.getElementAt(0, 2);
final var p23 = cameraMatrix.getElementAt(1, 2);
final var p33 = cameraMatrix.getElementAt(2, 2);
final var p14 = cameraMatrix.getElementAt(0, 3);
final var p24 = cameraMatrix.getElementAt(1, 3);
final var p34 = cameraMatrix.getElementAt(2, 3);
var residual = 0.0;
if (isPrincipalPointAtOrigin()) {
if (isZeroSkewness()) {
if (isFocalDistanceAspectRatioKnown()) {
// p2T*daq*p1 = 0
residual += residual2ndRowAnd1stRow(daq, p11, p21, p12, p22, p13, p23, p14, p24);
// p3T*daq*p1 = 0
residual += residual3rdRowAnd1stRow(daq, p11, p31, p12, p32, p13, p33, p14, p34);
// p3T*daw*p2 = 0
residual += residual3rdRowAnd2ndRow(daq, p21, p31, p22, p32, p23, p33, p24, p34);
// p1T*daq*p1*aspectRatio^2 = p2T*daq*p1
residual += residual1stRowEqualTo2ndRow(daq, p11, p21, p12, p22, p13, p23, p14, p24);
} else {
// p2T*daq*p1 = 0
residual += residual2ndRowAnd1stRow(daq, p11, p21, p12, p22, p13, p23, p14, p24);
// p3T*daq*p1 = 0
residual += residual3rdRowAnd1stRow(daq, p11, p31, p12, p32, p13, p33, p14, p34);
//p3T*daq*p2 = 0
residual += residual3rdRowAnd2ndRow(daq, p21, p31, p22, p32, p23, p33, p24, p34);
}
} else {
// p3T*daq*p1 = 0
residual += residual3rdRowAnd1stRow(daq, p11, p31, p12, p32, p13, p33, p14, p34);
// p3T*daq*p2 = 0
residual += residual3rdRowAnd2ndRow(daq, p21, p31, p22, p32, p23, p33, p24, p34);
}
} else {
return Double.MAX_VALUE;
}
return residual;
}
/**
* Computes residual for the equation p2T*daq*p1.
*
* @param daq estimated DAQ.
* @param p11 element (1,1) of camera matrix.
* @param p21 element (2,1) of camera matrix.
* @param p12 element (1,2) of camera matrix.
* @param p22 element (2,2) of camera matrix.
* @param p13 element (1,3) of camera matrix.
* @param p23 element (2,3) of camera matrix.
* @param p14 element (1,4) of camera matrix.
* @param p24 element (2,4) of camera matrix.
* @return obtained residual (ideally should be zero).
*/
private double residual2ndRowAnd1stRow(
final DualAbsoluteQuadric daq, final double p11, final double p21, final double p12, final double p22,
final double p13, final double p23, final double p14, final double p24) {
final var a = daq.getA();
final var b = daq.getB();
final var c = daq.getC();
final var d = daq.getD();
final var e = daq.getE();
final var f = daq.getF();
final var g = daq.getG();
final var h = daq.getH();
final var i = daq.getI();
final var j = daq.getJ();
return a * p21 * p11 + b * p22 * p12 + c * p23 * p13 + d * (p22 * p11 + p21 * p12)
+ e * (p23 * p12 + p22 * p13) + f * (p23 * p11 + p21 * p13)
+ g * (p24 * p11 + p21 * p14) + h * (p24 * p12 + p22 * p14)
+ i * (p24 * p13 + p23 * p14) + j * p24 * p14;
}
/**
* Computes residual for the equation p3T*daq*p1.
*
* @param daq estimated DAQ.
* @param p11 element (1,1) of camera matrix.
* @param p31 element (3,1) of camera matrix.
* @param p12 element (1,2) of camera matrix.
* @param p32 element (3,2) of camera matrix.
* @param p13 element (1,3) of camera matrix.
* @param p33 element (3,3) of camera matrix.
* @param p14 element (1,4) of camera matrix.
* @param p34 element (3,4) of camera matrix.
* @return obtained residual (ideally should be zero).
*/
private double residual3rdRowAnd1stRow(
final DualAbsoluteQuadric daq, final double p11, final double p31, final double p12, final double p32,
final double p13, final double p33, final double p14, final double p34) {
final var a = daq.getA();
final var b = daq.getB();
final var c = daq.getC();
final var d = daq.getD();
final var e = daq.getE();
final var f = daq.getF();
final var g = daq.getG();
final var h = daq.getH();
final var i = daq.getI();
final var j = daq.getJ();
return a * p31 * p11 + b * p32 * p12 + c * p33 * p13 + d * (p32 * p11 + p32 * p12)
+ e * (p33 * p12 + p32 * p13) + f * (p33 * p11 + p31 * p13)
+ g * (p34 * p11 + p31 * p14) + h * (p34 * p12 + p32 * p14)
+ i * (p34 * p13 + p13 * p14) + j * p34 * p14;
}
/**
* Computes residual for the equation p3T*daq*p2.
*
* @param daq estimated DAQ.
* @param p21 element (2,1) of camera matrix.
* @param p31 element (3,1) of camera matrix.
* @param p22 element (2,2) of camera matrix.
* @param p32 element (3,2) of camera matrix.
* @param p23 element (2,3) of camera matrix.
* @param p33 element (3,3) of camera matrix.
* @param p24 element (2,4) of camera matrix.
* @param p34 element (3,4) of camera matrix.
* @return obtained residual (ideally should be zero).
*/
private double residual3rdRowAnd2ndRow(
final DualAbsoluteQuadric daq, final double p21, final double p31, final double p22, final double p32,
final double p23, final double p33, final double p24, final double p34) {
final var a = daq.getA();
final var b = daq.getB();
final var c = daq.getC();
final var d = daq.getD();
final var e = daq.getE();
final var f = daq.getF();
final var g = daq.getG();
final var h = daq.getH();
final var i = daq.getI();
final var j = daq.getJ();
return a * p31 * p21 + b * p32 * p22 + c * p33 * p23 + d * (p32 * p21 + p31 * p22)
+ e * (p33 * p22 + p32 * p23) + f * (p33 * p21 + p31 * p23)
+ g * (p34 * p21 + p31 * p24) + h * (p34 * p22 + p32 * p24)
+ i * (p34 * p23 + p33 * p24) + j * p34 * p24;
}
/**
* Computes residual for the equation p1T*daq*p1 = r^2*p2T*daq*p2.
*
* @param daq estimated DAQ.
* @param p11 element (1,1) of camera matrix.
* @param p21 element (2,1) of camera matrix.
* @param p12 element (1,2) of camera matrix.
* @param p22 element (2,2) of camera matrix.
* @param p13 element (1,3) of camera matrix.
* @param p23 element (2,3) of camera matrix.
* @param p14 element (1,4) of camera matrix.
* @param p24 element (2,4) of camera matrix.
* @return obtained residual (ideally should be zero).
*/
private double residual1stRowEqualTo2ndRow(
final DualAbsoluteQuadric daq, final double p11, final double p21, final double p12, final double p22,
final double p13, final double p23, final double p14, final double p24) {
final var r = getFocalDistanceAspectRatio();
final var r2 = r * r;
final var a = daq.getA();
final var b = daq.getB();
final var c = daq.getC();
final var d = daq.getD();
final var e = daq.getE();
final var f = daq.getF();
final var g = daq.getG();
final var h = daq.getH();
final var i = daq.getI();
final var j = daq.getJ();
return a * (p11 * p11 * r2 - p21 * p21) + b * (p12 * p12 * r2 - p22 * p22)
+ c * (p13 * p13 * r2 - p23 * p23) + d * 2.0 * (p12 * p11 * r2 - p22 * p21)
+ e * 2.0 * (p13 * p12 * r2 - p23 * p22) + f * 2.0 * (p13 * p11 * r2 - p23 * p21)
+ g * 2.0 * (p14 * p11 * r2 - p24 * p21) + h * 2.0 * (p14 * p12 * r2 - p24 * p22)
+ i * 2.0 * (p14 * p13 * r2 - p24 * p23) + j * (p14 * p14 * r2 - p24 * p24);
}
/**
* Sets list of cameras.
* This method does not check whether estimator is locked.
*
* @param cameras list of cameras to estimate DAQ.
* @throws IllegalArgumentException if provided list of cameras is null
* or too small.
*/
private void internalSetCameras(final List<PinholeCamera> cameras) {
if (cameras == null || cameras.size() < getMinNumberOfRequiredCameras()) {
throw new IllegalArgumentException();
}
this.cameras = cameras;
}
}