ErrorOptimizationCameraCalibrator.java
/*
* Copyright (C) 2015 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;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.WrongSizeException;
import com.irurueta.ar.calibration.estimators.LMedSRadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.MSACRadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.PROMedSRadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.PROSACRadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.RANSACRadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.RadialDistortionRobustEstimator;
import com.irurueta.ar.calibration.estimators.RadialDistortionRobustEstimatorListener;
import com.irurueta.geometry.AxisRotation3D;
import com.irurueta.geometry.HomogeneousPoint2D;
import com.irurueta.geometry.HomogeneousPoint3D;
import com.irurueta.geometry.PinholeCamera;
import com.irurueta.geometry.Point2D;
import com.irurueta.geometry.Point3D;
import com.irurueta.geometry.Rotation3DType;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;
import com.irurueta.numerical.EvaluationException;
import com.irurueta.numerical.JacobianEstimator;
import com.irurueta.numerical.MultiVariateFunctionEvaluatorListener;
import com.irurueta.numerical.fitting.LevenbergMarquardtMultiVariateFitter;
import com.irurueta.numerical.fitting.LevenbergMarquardtMultiVariateFunctionEvaluator;
import com.irurueta.numerical.robust.RobustEstimatorMethod;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;
/**
* Calibrates a camera in order to find its intrinsic parameters and radial
* distortion by first estimating the intrinsic parameters without accounting
* for radial distortion and then use an optimization algorithm to minimize
* error and adjust estimated camera pose, intrinsic parameters and radial
* distortion parameters.
* <p>
* This class is based on technique described at:
* Zhengyou Zhang. A Flexible New Technique for Camera Calibration. Technical
* Report. MSR-TR-98-71. December 2, 1998.
*/
@SuppressWarnings("DuplicatedCode")
public class ErrorOptimizationCameraCalibrator extends CameraCalibrator {
/**
* Default robust estimator method to be used for radial distortion
* estimation.
*/
public static final RobustEstimatorMethod DEFAULT_RADIAL_DISTORTION_METHOD = RobustEstimatorMethod.PROSAC;
/**
* Indicates whether an initial radial distortion guess is estimated based
* on sampled data and estimated camera poses before starting the actual
* radial distortion optimization process.
*/
public static final boolean DEFAULT_ESTIMATE_INITIAL_RADIAL_DISTORTION = true;
/**
* Default maximum number of iterations to be used when adjusting parameters
* using Levenberg-Marquardt algorithm.
*/
public static final int DEFAULT_LEVENBERG_MARQUARDT_MAX_ITERS = 1000;
/**
* Default tolerance to assume that Levenberg-Marquardt algorithm has
* reached convergence when adjusting parameters.
*/
public static final double DEFAULT_LEVENBERG_MARQUARDT_TOLERANCE = 1e-12;
/**
* Maximum number of iterations to be used when adjusting parameters using
* Levenberg-Marquardt algorithm.
*/
private int levenbergMarquardtMaxIters;
/**
* Tolerance to assume that Levenberg-Marquardt algorithm has reached
* convergence when adjusting parameters.
*/
private double levenbergMarquardtTolerance;
/**
* Robust estimator method to be used for radial distortion estimation.
*/
private RobustEstimatorMethod distortionMethod;
/**
* Indicates whether an initial radial distortion guess is estimated based
* on sampled data and estimated camera poses before starting the actual
* radial distortion optimization process.
*/
private final boolean estimateInitialRadialDistortion;
/**
* Robust estimator of radial distortion.
*/
private RadialDistortionRobustEstimator distortionEstimator;
/**
* Listener for robust estimator of radial distortion.
*/
private RadialDistortionRobustEstimatorListener distortionEstimatorListener;
/**
* Indicates progress of radial distortion estimation.
*/
private float radialDistortionProgress;
/**
* Indicates progress of Levenberg-Marquardt fitting.
*/
private float fittingProgress;
/**
* Previously notified progress.
*/
private float previousNotifiedProgress;
/**
* Array to keep a relation between each point at index i-th and the sample
* (i.e. view) where it belongs to.
*/
private int[] indexToView;
/**
* Constructor.
*/
public ErrorOptimizationCameraCalibrator() {
super();
levenbergMarquardtMaxIters = DEFAULT_LEVENBERG_MARQUARDT_MAX_ITERS;
levenbergMarquardtTolerance = DEFAULT_LEVENBERG_MARQUARDT_TOLERANCE;
internalSetDistortionMethod(DEFAULT_RADIAL_DISTORTION_METHOD);
estimateInitialRadialDistortion = DEFAULT_ESTIMATE_INITIAL_RADIAL_DISTORTION;
}
/**
* Constructor.
*
* @param pattern 2D pattern to use for calibration.
* @param samples samples of the pattern taken with the camera to calibrate.
* @throws IllegalArgumentException if not enough samples are provided.
*/
public ErrorOptimizationCameraCalibrator(final Pattern2D pattern, final List<CameraCalibratorSample> samples) {
super(pattern, samples);
levenbergMarquardtMaxIters = DEFAULT_LEVENBERG_MARQUARDT_MAX_ITERS;
levenbergMarquardtTolerance = DEFAULT_LEVENBERG_MARQUARDT_TOLERANCE;
internalSetDistortionMethod(DEFAULT_RADIAL_DISTORTION_METHOD);
estimateInitialRadialDistortion = DEFAULT_ESTIMATE_INITIAL_RADIAL_DISTORTION;
}
/**
* Constructor.
*
* @param pattern 2D pattern to use for calibration.
* @param samples samples of the pattern taken with the camera to calibrate.
* @param samplesQualityScores quality scores for each sample.
* @throws IllegalArgumentException if not enough samples are provided or
* both samples and quality scores do not have the same size.
*/
public ErrorOptimizationCameraCalibrator(
final Pattern2D pattern, final List<CameraCalibratorSample> samples, final double[] samplesQualityScores) {
super(pattern, samples, samplesQualityScores);
levenbergMarquardtMaxIters = DEFAULT_LEVENBERG_MARQUARDT_MAX_ITERS;
levenbergMarquardtTolerance = DEFAULT_LEVENBERG_MARQUARDT_TOLERANCE;
internalSetDistortionMethod(DEFAULT_RADIAL_DISTORTION_METHOD);
estimateInitialRadialDistortion = DEFAULT_ESTIMATE_INITIAL_RADIAL_DISTORTION;
}
/**
* Returns maximum number of iterations to be used when adjusting parameters
* using Levenberg-Marquardt algorithm.
*
* @return maximum number of iterations to be used when adjusting parameters
* using Levenberg-Marquardt algorithm.
*/
public int getLevenbergMarquardtMaxIters() {
return levenbergMarquardtMaxIters;
}
/**
* Sets maximum number of iterations to be used when adjusting parameters
* using Levenberg-Marquardt algorithm.
*
* @param levenbergMarquardtMaxIters maximum number of iterations to be used
* when adjusting parameters using Levenberg-Marquardt algorithm.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if this instance is locked.
*/
public void setLevenbergMarquardtMaxIters(final int levenbergMarquardtMaxIters) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (levenbergMarquardtMaxIters <= 0) {
throw new IllegalArgumentException();
}
this.levenbergMarquardtMaxIters = levenbergMarquardtMaxIters;
}
/**
* Returns tolerance to assume that Levenberg-Marquardt algorithm has
* reached convergence when adjusting parameters.
*
* @return tolerance to assume that Levenberg-Marquardt algorithm has
* reached convergence when adjusting parameters.
*/
public double getLevenbergMarquardtTolerance() {
return levenbergMarquardtTolerance;
}
/**
* Sets tolerance to assume that Levenberg-Marquardt algorithm has reached
* convergence when adjusting parameters.
*
* @param levenbergMarquardtTolerance tolerance to assume that
* Levenberg-Marquardt algorithm has reached convergence when
* adjusting parameter.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if this instance is locked.
*/
public void setLevenbergMarquardtTolerance(final double levenbergMarquardtTolerance) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (levenbergMarquardtTolerance <= 0.0) {
throw new IllegalArgumentException();
}
this.levenbergMarquardtTolerance = levenbergMarquardtTolerance;
}
/**
* Returns robust estimator method to be used for radial distortion
* estimation.
*
* @return robust estimator method to be used for radial distortion
* estimation.
*/
public RobustEstimatorMethod getDistortionMethod() {
return distortionMethod;
}
/**
* Sets robust estimator method to be used for radial distortion
* estimation.
*
* @param distortionMethod robust estimator method to be used for
* radial distortion estimation.
* @throws LockedException if this instance is locked.
*/
public void setDistortionMethod(final RobustEstimatorMethod distortionMethod) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
internalSetDistortionMethod(distortionMethod);
}
/**
* Returns radial distortion estimator, which can be retrieved in case
* that some additional parameter needed to be adjusted.
* It is discouraged to directly access the distortion estimator during
* camera calibration, as it might interfere with the results.
*
* @return radial distortion estimator.
*/
public RadialDistortionRobustEstimator getDistortionEstimator() {
return distortionEstimator;
}
/**
* Returns threshold to robustly estimate radial distortion.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
*
* @return threshold to robustly estimate radial distortion.
*/
public double getDistortionEstimatorThreshold() {
return switch (distortionEstimator.getMethod()) {
case LMEDS -> ((LMedSRadialDistortionRobustEstimator) distortionEstimator).getStopThreshold();
case MSAC -> ((MSACRadialDistortionRobustEstimator) distortionEstimator).getThreshold();
case PROSAC -> ((PROSACRadialDistortionRobustEstimator) distortionEstimator).getThreshold();
case PROMEDS -> ((PROMedSRadialDistortionRobustEstimator) distortionEstimator).getStopThreshold();
default -> ((RANSACRadialDistortionRobustEstimator) distortionEstimator).getThreshold();
};
}
/**
* Sets threshold to robustly estimate radial distortion.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finder adjustments.
*
* @param distortionEstimatorThreshold threshold to robustly estimate
* radial distortion .
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided value is zero or negative.
*/
public void setDistortionEstimatorThreshold(final double distortionEstimatorThreshold) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
switch (distortionEstimator.getMethod()) {
case LMEDS:
((LMedSRadialDistortionRobustEstimator) distortionEstimator).setStopThreshold(
distortionEstimatorThreshold);
break;
case MSAC:
((MSACRadialDistortionRobustEstimator) distortionEstimator).setThreshold(distortionEstimatorThreshold);
break;
case PROSAC:
((PROSACRadialDistortionRobustEstimator) distortionEstimator).setThreshold(
distortionEstimatorThreshold);
break;
case PROMEDS:
((PROMedSRadialDistortionRobustEstimator) distortionEstimator).setStopThreshold(
distortionEstimatorThreshold);
break;
case RANSAC:
default:
((RANSACRadialDistortionRobustEstimator) distortionEstimator).setThreshold(
distortionEstimatorThreshold);
break;
}
}
/**
* Returns confidence to robustly estimate radial distortion.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
* Confidence is expressed as a value between 0.0 (0%) and 1.0 (100%). The
* amount of confidence indicates the probability that the estimated
* homography is correct (i.e. no outliers were used for the estimation,
* because they were successfully discarded).
* Typically, this value will be close to 1.0, but not exactly 1.0, because
* a 100% confidence would require an infinite number of iterations.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
*
* @return confidence to robustly estimate homographies.
*/
public double getDistortionEstimatorConfidence() {
return distortionEstimator.getConfidence();
}
/**
* Sets confidence to robustly estimate radial distortion.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
* Confidence is expressed as a value between 0.0 (0%) and 1.0 (100%). The
* amount of confidence indicates the probability that the estimated
* homography is correct (i.e. no outliers were used for the estimation,
* because they were successfully discarded).
* Typically, this value will be close to 1.0, but not exactly 1.0, because
* a 100% confidence would require an infinite number of iterations.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
*
* @param distortionEstimatorConfidence confidence to robustly estimate
* radial distortion.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided value is not between 0.0 and
* 1.0.
*/
public void setDistortionEstimatorConfidence(final double distortionEstimatorConfidence) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
distortionEstimator.setConfidence(distortionEstimatorConfidence);
}
/**
* Returns the maximum number of iterations to be done when estimating
* the radial distortion.
* If the maximum allowed number of iterations is reached, resulting
* estimation might not have desired confidence.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
*
* @return maximum number of iterations to be done when estimating the
* homographies.
*/
public int getDistortionEstimatorMaxIterations() {
return distortionEstimator.getMaxIterations();
}
/**
* Sets the maximum number of iterations to be done when estimating the
* radial distortion.
* If the maximum allowed number of iterations is reached, resulting
* estimation might not have desired confidence.
* Usually the default value is good enough for most situations, but this
* setting can be changed for finer adjustments.
*
* @param distortionEstimatorMaxIterations maximum number of iterations to
* be done when estimating radial distortion.
* @throws LockedException if this instance is locked.
* @throws IllegalArgumentException if provided value is negative or zero.
*/
public void setDistortionEstimatorMaxIterations(final int distortionEstimatorMaxIterations) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
distortionEstimator.setMaxIterations(distortionEstimatorMaxIterations);
}
/**
* Starts the calibration process.
* Depending on the settings the following will be estimated:
* intrinsic pinhole camera parameters, radial distortion of lens,
* camera pose (rotation and translation) for each sample, and the
* associated homobraphy of sampled points respect to the ideal pattern
* samples.
*
* @throws CalibrationException if calibration fails for some reason.
* @throws LockedException if this instance is locked because calibration is
* already in progress.
* @throws NotReadyException if this instance does not have enough data to
* start camera calibration.
*/
@Override
public void calibrate() throws CalibrationException, LockedException, NotReadyException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
locked = true;
homographyQualityScoresRequired = (distortionEstimator.getMethod() == RobustEstimatorMethod.PROSAC
|| distortionEstimator.getMethod() == RobustEstimatorMethod.PROMEDS);
if (listener != null) {
listener.onCalibrateStart(this);
}
reset();
radialDistortionProgress = fittingProgress = previousNotifiedProgress = 0.0f;
final var idealFallbackPatternMarkers = pattern.getIdealPoints();
try {
// estimate intrinsic parameters
estimateIntrinsicParameters(idealFallbackPatternMarkers);
if (estimateRadialDistortion) {
// estimate radial distortion
estimateRadialDistortion(idealFallbackPatternMarkers);
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} finally {
locked = false;
}
}
/**
* Returns the camera calibrator method used by this instance.
*
* @return the camera calibrator method.
*/
@Override
public CameraCalibratorMethod getMethod() {
return CameraCalibratorMethod.ERROR_OPTIMIZATION;
}
/**
* Notifies progress to current listener, if needed.
*/
@Override
protected void notifyProgress() {
final float progress;
if (estimateInitialRadialDistortion) {
progress = (radialDistortionProgress + intrinsicProgress + fittingProgress) / 3.0f;
} else {
progress = 0.5f * intrinsicProgress + 0.5f * fittingProgress;
}
if (listener != null && (progress - previousNotifiedProgress) > progressDelta) {
listener.onCalibrateProgressChange(this, progress);
previousNotifiedProgress = progress;
}
}
/**
* Estimates radial distortion by minimizing the re-projection error by
* adjusting the camera pose and radial distortion parameters using an
* optimization algorithm.
* The initial solution for the optimization algorithm is the estimated
* camera pose and intrinsic parameters without accounting for radial
* distortion and radial distortion parameters equal to 0.0.
*
* @param idealFallbackPatternMarkers ideal pattern markers coordinates.
* These coordinates are used as fallback when a given sample
* does not have an associated pattern.
* @return average re-projection error, obtained after projecting ideal
* pattern markers using estimated camera poses and then doing a comparison
* with sampled points taking into account estimated distortion to undo
* their corresponding distortion.
* @throws CalibrationException if anything fails.
*/
protected double estimateRadialDistortion(final List<Point2D> idealFallbackPatternMarkers)
throws CalibrationException {
radialDistortionProgress = 0.0f;
if (listener != null) {
listener.onRadialDistortionEstimationStarts(this);
}
// compute total points for samples where homography could be estimated
var totalPoints = 0;
var totalHomographies = 0;
for (final var sample : samples) {
if (sample.getHomography() != null) {
totalPoints += sample.getSampledMarkers().size();
totalHomographies++;
}
}
indexToView = new int[totalPoints];
if (estimateInitialRadialDistortion) {
final var distortedPoints = new ArrayList<Point2D>();
final var undistortedPoints = new ArrayList<Point2D>();
double[] qualityScores = null;
if (distortionMethod == RobustEstimatorMethod.PROSAC || distortionMethod == RobustEstimatorMethod.PROMEDS) {
qualityScores = new double[totalPoints];
}
// estimate camera pose for each sample
var pointCounter = 0;
var sampleCounter = 0;
for (final var sample : samples) {
if (sample.getHomography() == null) {
// homography computation failed, so we cannot compute camera
// pose for this sample
continue;
}
sample.computeCameraPose(intrinsic);
// transform ideal pattern markers using estimated homography
final List<Point2D> idealPatternMarkers;
if (sample.getPattern() != null) {
// use points generated by pattern in sample
idealPatternMarkers = sample.getPattern().getIdealPoints();
} else {
// use fallback pattern points
idealPatternMarkers = idealFallbackPatternMarkers;
}
final var transformedIdealPatternMarkers = sample.getHomography().transformPointsAndReturnNew(
idealPatternMarkers);
distortedPoints.addAll(sample.getSampledMarkers());
undistortedPoints.addAll(transformedIdealPatternMarkers);
final var markersSize = transformedIdealPatternMarkers.size();
// fills array indicating to which sample (i.e. view) each point
// belongs to
Arrays.fill(indexToView, pointCounter, pointCounter + markersSize, sampleCounter);
// if distortion estimator requires quality scores, set them
if (qualityScores != null && (distortionMethod == RobustEstimatorMethod.PROSAC
|| distortionMethod == RobustEstimatorMethod.PROMEDS)) {
final var sampleQuality = homographyQualityScores[sampleCounter];
// assign to all points (markers) in the sample the same sample
// quality
for (var i = pointCounter; i < pointCounter + markersSize; i++) {
qualityScores[i] = sampleQuality;
}
pointCounter += markersSize;
sampleCounter++;
}
}
// estimate radial distortion
try {
distortionEstimator.setIntrinsic(intrinsic);
distortionEstimator.setPoints(distortedPoints, undistortedPoints);
distortionEstimator.setQualityScores(qualityScores);
distortion = distortionEstimator.estimate();
} catch (final Exception e) {
throw new CalibrationException(e);
}
} else {
// estimate camera pose for each sample
for (final var sample : samples) {
if (sample.getHomography() == null) {
// homography computation failed, so we cannot compute camera
// pose for this sample
continue;
}
sample.computeCameraPose(intrinsic);
}
// set initial radial distortion as if there was no distortion
distortion = new RadialDistortion(0.0, 0.0);
}
// optimize cost function to refine camera poses and radial distortion
try {
// compute initial parameters to fit a function using
// Levenberg-Marquardt
final var initParams = new double[numParameters(totalHomographies)];
paramsFromData(initParams);
// compute x data (input points)
final var x = dataXToMatrix(idealFallbackPatternMarkers);
// compute y data (output points)
final var y = dataYToMatrix();
// Evaluator for Levenberg-Marquardt fitter. This is in charge of
// evaluating function to be fitted using current parameters and
// also in charge of estimating function Jacobian for each point
// where the function is evaluated
final var evaluator = new LevenbergMarquardtMultiVariateFunctionEvaluator() {
// position of current point being evaluated
private int i;
// current point being evaluated
private double[] point;
// Instance in charge of estimating Jacobian of function being
// fitted at current point and for provided parameters. Jacobian
// is computed by keeping point constant and computing the
// partial derivatives for each parameter
private final JacobianEstimator jacobianEstimator = new JacobianEstimator(
new MultiVariateFunctionEvaluatorListener() {
// We provide params so that jacobian is computed as the
// partial derivatives respect parameters
@Override
public void evaluate(final double[] params, final double[] result) {
evaluateFunction(i, point, params, result);
}
// Function being fitted is multi variate returning 2D points
// (having horizontal and vertical inhomogeneous coordinates)
@Override
public int getNumberOfVariables() {
return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
}
});
// Function being fitted has as input data 2D points (having
// horizontal and vertical inhomogeneous coordinates)
@Override
public int getNumberOfDimensions() {
return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
}
// Function being fitted is multi variate returning 2D points
// (having horizontal and vertical inhomogeneous coordinates)
@Override
public int getNumberOfVariables() {
return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
}
// Creates array where parameters are stored. This array is
// initialized with the parameter values initially used by the
// Levenberg-Marquardt algorithm
@Override
public double[] createInitialParametersArray() {
return initParams;
}
// Evaluates function to be fitted and computes Jacobian
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) throws EvaluationException {
this.i = i;
this.point = point;
evaluateFunction(this.i, this.point, params, result);
jacobianEstimator.jacobian(params, jacobian);
}
};
// fits function
final var sigma = 1.0;
final var fitter = new LevenbergMarquardtMultiVariateFitter(evaluator, x, y, sigma);
fitter.setItmax(levenbergMarquardtMaxIters);
fitter.setTol(levenbergMarquardtTolerance);
final var estimatedParams = fitter.getA();
// updates camera poses from estimated parameters
dataFromParams(estimatedParams);
// computes re-projection errors between sampled and ideal data using
// fitted parameters
final var error = computeReprojectionError(idealFallbackPatternMarkers);
if (listener != null) {
listener.onRadialDistortionEstimationEnds(this, distortion);
}
return error;
} catch (Exception e) {
throw new CalibrationException(e);
}
}
/**
* Refreshes listener of distortion estimator when robust estimator method
* is changed for the distortion estimator.
*/
protected void refreshDistortionEstimatorListener() {
if (distortionEstimatorListener == null) {
distortionEstimatorListener = new RadialDistortionRobustEstimatorListener() {
@Override
public void onEstimateStart(final RadialDistortionRobustEstimator estimator) {
radialDistortionProgress = 0.0f;
notifyProgress();
}
@Override
public void onEstimateEnd(final RadialDistortionRobustEstimator estimator) {
radialDistortionProgress = 1.0f;
notifyProgress();
}
@Override
public void onEstimateNextIteration(
final RadialDistortionRobustEstimator estimator, final int iteration) {
// not used
}
@Override
public void onEstimateProgressChange(
final RadialDistortionRobustEstimator estimator, final float progress) {
radialDistortionProgress = progress;
notifyProgress();
}
};
}
try {
distortionEstimator.setListener(distortionEstimatorListener);
} catch (final LockedException e) {
Logger.getLogger(AlternatingCameraCalibrator.class.getName()).log(Level.WARNING,
"Could not set radial distortion estimator listener", e);
}
}
/**
* Evaluates provided point and parameters to obtain the distorted points
* that would be obtained. Levenberg-Marquardt algorithm will iteratively
* change parameters until the obtained result approximates sampled Y data
*
* @param i index of point among all provided data.
* @param point input point to be evaluated.
* @param params parameters to evaluate function.
* @param result result of evaluation.
*/
private void evaluateFunction(final int i, final double[] point, final double[] params, final double[] result) {
// set data from current params (updates camera poses for each sample -
// i.e. view)
dataFromParams(params);
final var numView = indexToView[i];
// obtain camera pose for numView
final var sample = samples.get(numView);
final var camera = sample.getCamera();
final var idealPoint3D = new HomogeneousPoint3D();
// ideal 3D point is the marker point assumed to be at plane z = 0
idealPoint3D.setInhomogeneousCoordinates(point[0], point[1], 0.0);
// project ideal point using estimated camera
final var undistortedPoint = camera.project(idealPoint3D);
// add distortion
final var distortedPoint = new HomogeneousPoint2D();
distortion.distort(undistortedPoint, distortedPoint);
result[0] = distortedPoint.getInhomX();
result[1] = distortedPoint.getInhomY();
}
/**
* Converts undistorted points corresponding to ideal pattern markers into
* a matrix to be used by Levenberg-Marquardt as the input data to be used
* by the function being fitted.
*
* @param idealFallbackPatternMarkers ideal pattern markers coordinates.
* These coordinates are used as fallback when a given sample
* does not have an associated pattern.
* @return a matrix.
* @throws WrongSizeException if no undistorted points are available.
*/
private Matrix dataXToMatrix(final List<Point2D> idealFallbackPatternMarkers) throws WrongSizeException {
final var idealPoints = new ArrayList<Point2D>();
for (final var sample : samples) {
final List<Point2D> idealPatternMarkers;
if (sample.getPattern() != null) {
// use points generated by pattern in sample
idealPatternMarkers = sample.getPattern().getIdealPoints();
} else {
// use fallback pattern points
idealPatternMarkers = idealFallbackPatternMarkers;
}
idealPoints.addAll(idealPatternMarkers);
}
final var nPoints = idealPoints.size();
final var m = new Matrix(nPoints, Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH);
var i = 0;
for (final var sample : samples) {
final List<Point2D> idealPatternMarkers;
if (sample.getPattern() != null) {
// use points generated by pattern in sample
idealPatternMarkers = sample.getPattern().getIdealPoints();
} else {
// use fallback pattern points
idealPatternMarkers = idealFallbackPatternMarkers;
}
for (final var point : idealPatternMarkers) {
m.setElementAt(i, 0, point.getInhomX());
m.setElementAt(i, 1, point.getInhomY());
i++;
}
}
return m;
}
/**
* Converts sampled distorted points into a matrix to be used by
* Levenberg-Marquardt algorithm as the sampled function evaluations.
*
* @return a matrix.
* @throws WrongSizeException if no sampled points are available.
*/
private Matrix dataYToMatrix() throws WrongSizeException {
var nPoints = 0;
for (final var sample : samples) {
nPoints += sample.getSampledMarkers().size();
}
final var m = new Matrix(nPoints, Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH);
var i = 0;
for (final var sample : samples) {
for (final var point : sample.getSampledMarkers()) {
m.setElementAt(i, 0, point.getInhomX());
m.setElementAt(i, 1, point.getInhomY());
i++;
}
}
return m;
}
/**
* Sets parameters of function to be fitted using Levenberg-Marquardt
* algorithm.
* These parameters will be used as an initial solution and on each
* iteration of the Levenberg-Marquardt algorithm.
*
* @param params arrays where parameters will be set using current data.
*/
private void paramsFromData(final double[] params) {
var pos = 0;
// common parameters (intrinsic camera parameters and radial distortion
// parameters)
// intrinsic parameters
if (!isZeroSkewness()) {
// aspect ratio is not known (2 different focal distances) and
// skewness is not zero
params[pos] = intrinsic.getHorizontalFocalLength();
pos++;
params[pos] = intrinsic.getVerticalFocalLength();
pos++;
params[pos] = intrinsic.getSkewness();
pos++;
} else {
// skewness is always zero (so it is not stored in vector)
params[pos] = intrinsic.getHorizontalFocalLength();
pos++;
if (!isFocalDistanceAspectRatioKnown()) {
// focal distances are different, so we also store vertical
// one
params[pos] = intrinsic.getVerticalFocalLength();
pos++;
}
}
if (!isPrincipalPointAtOrigin()) {
// principal point is not zero
params[pos] = intrinsic.getHorizontalPrincipalPoint();
pos++;
params[pos] = intrinsic.getVerticalPrincipalPoint();
pos++;
}
// radial distortion parameters
final var kParams = distortion.getKParams();
for (final var kParam : kParams) {
params[pos] = kParam;
pos++;
}
// parameters for each sample (camera rotation and translation)
for (final var sample : samples) {
if (sample.getHomography() == null) {
continue;
}
// 4 rotation parameters
AxisRotation3D rot;
if (sample.getRotation().getType() == Rotation3DType.AXIS_ROTATION3D) {
rot = (AxisRotation3D) sample.getRotation();
} else {
rot = new AxisRotation3D(sample.getRotation());
}
params[pos] = rot.getRotationAngle();
pos++;
params[pos] = rot.getAxisX();
pos++;
params[pos] = rot.getAxisY();
pos++;
params[pos] = rot.getAxisZ();
pos++;
// 3 translation parameters (camera center)
final var center = sample.getCameraCenter();
params[pos] = center.getInhomX();
pos++;
params[pos] = center.getInhomY();
pos++;
params[pos] = center.getInhomZ();
pos++;
}
}
/**
* Sets data in samples from parameters values fitted by the
* Levenberg-Marquardt algorithm.
*
* @param params vector containing estimated parameters .
*/
private void dataFromParams(final double[] params) {
var pos = 0;
// intrinsic parameters
double horizontalFocalLength;
double verticalFocalLength;
double skewness;
double horizontalPrincipalPoint;
double verticalPrincipalPoint;
if (!isZeroSkewness()) {
// aspect ratio is not known (2 different focal distances) and
// skewness is not zero
horizontalFocalLength = params[pos];
pos++;
verticalFocalLength = params[pos];
pos++;
skewness = params[pos];
pos++;
} else {
// skewness is always zero (so it is not stored in vector)
skewness = 0.0;
horizontalFocalLength = params[pos];
pos++;
if (!isFocalDistanceAspectRatioKnown()) {
// focal distances are different
verticalFocalLength = params[pos];
pos++;
} else {
// vertical focal distance is related to horizontal one
// through aspect ratio
verticalFocalLength = horizontalFocalLength * getFocalDistanceAspectRatio();
}
}
if (!isPrincipalPointAtOrigin()) {
// principal point is not zero
horizontalPrincipalPoint = params[pos];
pos++;
verticalPrincipalPoint = params[pos];
pos++;
} else {
// principal point is zero
horizontalPrincipalPoint = verticalPrincipalPoint = 0.0;
}
// update intrinsic parameters
intrinsic.setHorizontalFocalLength(horizontalFocalLength);
intrinsic.setVerticalFocalLength(verticalFocalLength);
intrinsic.setSkewness(skewness);
intrinsic.setHorizontalPrincipalPoint(horizontalPrincipalPoint);
intrinsic.setVerticalPrincipalPoint(verticalPrincipalPoint);
// radial distortion parameters
final var kParams = distortion.getKParams();
for (var i = 0; i < kParams.length; i++) {
kParams[i] = params[pos];
pos++;
}
// sample parameters
for (final var sample : samples) {
if (sample.getHomography() == null) {
continue;
}
// 4 rotation parameters
final AxisRotation3D rot;
if (sample.getRotation().getType() == Rotation3DType.AXIS_ROTATION3D) {
rot = (AxisRotation3D) sample.getRotation();
} else {
rot = new AxisRotation3D();
// update sample
sample.setRotation(rot);
}
final var rotAngle = params[pos];
pos++;
final var axisX = params[pos];
pos++;
final var axisY = params[pos];
pos++;
final var axisZ = params[pos];
pos++;
rot.setAxisAndRotation(axisX, axisY, axisZ, rotAngle);
// 3 translation parameters (camera center)
final var inhomX = params[pos];
pos++;
final var inhomY = params[pos];
pos++;
final var inhomZ = params[pos];
pos++;
final var center = sample.getCameraCenter();
center.setInhomogeneousCoordinates(inhomX, inhomY, inhomZ);
// update camera
final var camera = sample.getCamera();
camera.setIntrinsicAndExtrinsicParameters(intrinsic, rot, center);
}
}
/**
* Computes re-projection error taking into account ideal pattern marker
* points, transforming them using estimated homography, adding to them
* distortion and comparing them with sampled points.
*
* @param idealFallbackPatternMarkers ideal 2D pattern marker points used
* as fallback in case that a given sample does not have an
* associated pattern.
* @return average re-projection error.
*/
private double computeReprojectionError(final List<Point2D> idealFallbackPatternMarkers) {
// distorted points are the sampled points
// undistorted points are the ideal pattern marker points projected
// using current camera pose
PinholeCamera camera;
Point2D marker2D;
final var marker3D = Point3D.create();
final var undistortedPoint = Point2D.create();
var totalPoints = 0;
final var distortedPoint = Point2D.create();
Point2D sampledPoint;
var avgError = 0.0;
for (final var sample : samples) {
camera = sample.getCamera();
if (camera == null) {
continue;
}
final List<Point2D> idealPatternMarkers;
if (sample.getPattern() != null) {
idealPatternMarkers = sample.getPattern().getIdealPoints();
} else {
idealPatternMarkers = idealFallbackPatternMarkers;
}
final var pointsPerSample = idealPatternMarkers.size();
for (var i = 0; i < pointsPerSample; i++) {
marker2D = idealPatternMarkers.get(i);
sampledPoint = sample.getSampledMarkers().get(i);
// convert ideal marker point into a 3D point placed in plane
// z = 0
marker3D.setInhomogeneousCoordinates(marker2D.getInhomX(), marker2D.getInhomY(), 0.0);
// project 3D marker point using estimated camera on current
// sample (i.e. view)
camera.project(marker3D, undistortedPoint);
// add distortion to ideal projected point
distortion.distort(undistortedPoint, distortedPoint);
// obtain distance between sampled point and ideal projected
// point with added distortion
avgError += sampledPoint.distanceTo(distortedPoint);
totalPoints++;
}
}
if (totalPoints == 0) {
avgError = Double.MAX_VALUE;
} else {
avgError /= totalPoints;
}
return avgError;
}
/**
* Sets robust estimator method to be used for radial distortion estimation.
* If method changes, then a new radial distortion estimator is created and
* configured.
*
* @param distortionMethod robust estimator method to be used for
* radial distortion estimation.
*/
private void internalSetDistortionMethod(RobustEstimatorMethod distortionMethod) {
// if method changes, recreate estimator
if (distortionMethod != this.distortionMethod) {
final var previousAvailable = this.distortionMethod != null;
var threshold = 0.0;
var confidence = 0.0;
var maxIterations = 0;
if (previousAvailable) {
threshold = getDistortionEstimatorThreshold();
confidence = getDistortionEstimatorConfidence();
maxIterations = getDistortionEstimatorMaxIterations();
}
distortionEstimator = RadialDistortionRobustEstimator.create(distortionMethod);
// configure new estimator
refreshDistortionEstimatorListener();
if (previousAvailable) {
try {
setDistortionEstimatorThreshold(threshold);
setDistortionEstimatorConfidence(confidence);
setDistortionEstimatorMaxIterations(maxIterations);
} catch (final LockedException e) {
Logger.getLogger(AlternatingCameraCalibrator.class.getName()).log(Level.WARNING,
"Could not reconfigure distortion estimator", e);
}
}
}
this.distortionMethod = distortionMethod;
}
/**
* Returns number of parameters of cost function.
*
* @param numHomographies number of valid estimated homographies.
* @return number of parameters of cost function.
*/
private int numParameters(final int numHomographies) {
// For each homography there are:
// - 4 rotation parameters (angle and axis coordinates x, y, z)
// - 3 translation parameters
// - x intrinsic parameters (depending on settings)
// - x radial distortion parameters (K params length)
return 7 * numHomographies + numIntrinsicParameters() + distortion.getKParams().length;
}
/**
* Returns number of intrinsic parameters to be taken into account in
* cost function.
*
* @return number of intrinsic parameters to be taken into account in
* cost function.
*/
private int numIntrinsicParameters() {
// if no constraints, there are 5 intrinsic parameters (horizontal
// focal length, vertical focal length, skewness, horizontal principal
// point and vertical principal point
var num = 5;
if (isZeroSkewness()) {
if (isFocalDistanceAspectRatioKnown()) {
num--;
}
num--;
}
if (isPrincipalPointAtOrigin()) {
num -= 2;
}
return num;
}
}