MSACRobustEstimator.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.numerical.robust;
import com.irurueta.numerical.LockedException;
import com.irurueta.numerical.NotReadyException;
import com.irurueta.sorting.Sorter;
import java.util.ArrayList;
import java.util.BitSet;
/**
* This class implements MSAC (Median SAmple Consensus) algorithm to robustly
* estimate a data model.
* MSAC is a mixture between LMedS and RANSAC, where a fixed threshold is
* used such as in RANSAC to determine the number of remaining iterations, and
* the least median of residuals is used to pick the best solution, rather than
* the one producing a higher number of inliers based on the fixed threshold,
* such as in RANSAC.
* This algorithm requires a threshold known beforehand such as RANSAC, but
* might get better accuracy if the inlier samples are very accurate, since
* the solution with the smallest median of residuals will be picked. In typical
* situations however, this algorithm will produce similar results to RANSAC in
* both terms of accuracy and computational cost, since typically inlier samples
* tend to have certain error.
*
* @param <T> type of object to be estimated.
*/
public class MSACRobustEstimator<T> extends RobustEstimator<T> {
/**
* Constant defining default confidence of 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;
/**
* Minimum allowed threshold to determine inliers.
*/
public static final double MIN_THRESHOLD = 0.0;
/**
* Amount of confidence expressed as a value between 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.
*/
private 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.
*/
private int maxIterations;
/**
* Instance in charge of picking random subsets of samples.
*/
private SubsetSelector subsetSelector;
/**
* Number of iterations to be done to obtain required confidence.
*/
private int iters;
/**
* Best solution that has been found so far during an estimation.
*/
private T bestResult;
/**
* Data related to inliers found for best result.
*/
private MSACInliersData bestResultInliersData;
/**
* Data related to solution producing the largest number of inliers.
*/
private MSACInliersData bestNumberInliersData;
/**
* Constructor.
*/
public MSACRobustEstimator() {
super();
confidence = DEFAULT_CONFIDENCE;
maxIterations = DEFAULT_MAX_ITERATIONS;
iters = maxIterations;
bestResult = null;
bestResultInliersData = bestNumberInliersData = null;
}
/**
* Constructor.
*
* @param listener listener to handle events raised by this estimator.
*/
public MSACRobustEstimator(final MSACRobustEstimatorListener<T> listener) {
super(listener);
confidence = DEFAULT_CONFIDENCE;
maxIterations = DEFAULT_MAX_ITERATIONS;
iters = maxIterations;
bestResult = null;
bestResultInliersData = bestNumberInliersData = null;
}
/**
* Returns amount of confidence expressed as a value between 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 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 estimation
* 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;
}
/**
* 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.
*
* @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;
}
/**
* Returns number of iterations to be done to obtain required confidence.
*
* @return number of iterations to be done to obtain required confidence.
*/
public int getNIters() {
return iters;
}
/**
* Returns best solution that has been found so far during an estimation.
*
* @return best solution that has been found so far during an estimation.
*/
public T getBestResult() {
return bestResult;
}
/**
* Returns data related to the best inliers found for best result.
*
* @return data related to inliers found for best result.
*/
public MSACInliersData getBestResultInliersData() {
return bestResultInliersData;
}
/**
* Returns data related to solution producing the largest number of inliers.
*
* @return data related to solution producing the largest number of inliers.
*/
public MSACInliersData getBestNumberInliersData() {
return bestNumberInliersData;
}
/**
* Indicates if estimator is ready to start the estimation process.
*
* @return true if ready, false otherwise.
*/
@Override
public boolean isReady() {
if (!super.isReady()) {
return false;
}
return (listener instanceof MSACRobustEstimatorListener);
}
/**
* Robustly estimates an instance of T.
*
* @return estimated object.
* @throws LockedException if robust estimator is locked.
* @throws NotReadyException if provided input data is not enough to start
* the estimation.
* @throws RobustEstimatorException if estimation fails for any reason
* (i.e. numerical instability, no solution available, etc).
*/
@Override
@SuppressWarnings("Duplicates")
public T estimate() throws LockedException, NotReadyException, RobustEstimatorException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
final var listener = (MSACRobustEstimatorListener<T>) this.listener;
locked = true;
listener.onEstimateStart(this);
final var totalSamples = listener.getTotalSamples();
final var subsetSize = listener.getSubsetSize();
final var threshold = listener.getThreshold();
// only positive thresholds are allowed
if (threshold < MIN_THRESHOLD) {
throw new RobustEstimatorException();
}
var bestNumInliers = 0;
var bestMedianResidual = Double.MAX_VALUE;
iters = Integer.MAX_VALUE;
int newNIters;
var currentIter = 0;
// reusable list that will contain preliminary solutions on each
// iteration
final var iterResults = new ArrayList<T>();
bestResult = null; // best result found so far
int currentInliers;
// progress and previous progress to determine when progress
// notification must occur
var previousProgress = 0.0f;
float progress;
// indices of subset picked in one iteration
final var subsetIndices = new int[subsetSize];
final var residualsTemp = new double[totalSamples];
if (subsetSelector == null) {
// create new subset selector
subsetSelector = SubsetSelector.create(totalSamples);
} else {
// set number of samples to current subset selector
subsetSelector.setNumSamples(totalSamples);
}
// data related to inliers
var inliersData = new MSACInliersData(totalSamples);
// sorter to compute medians
final var sorter = Sorter.<Double>create();
while ((iters > currentIter) && (currentIter < maxIterations)) {
// generate a random subset of samples
subsetSelector.computeRandomSubsets(subsetSize, subsetIndices);
// clear list of preliminary solutions before calling listener
iterResults.clear();
// compute solution for current iteration
listener.estimatePreliminarSolutions(subsetIndices, iterResults);
// iterate over all solutions that have been found
for (final var iterResult : iterResults) {
// compute inliers
computeInliers(iterResult, threshold, residualsTemp, listener, sorter, inliersData);
// save solution that minimizes the median residual
if (inliersData.isMedianResidualImproved()) {
// keep current solution
bestResult = iterResult;
// keep the best inliers data corresponding to best solution
// in case it can be useful along with the result
bestResultInliersData = inliersData;
bestMedianResidual = inliersData.getBestMedianResidual();
}
// if number of inliers have improved, update number of
// remaining iterations
currentInliers = inliersData.getNumInliers();
if (currentInliers > bestNumInliers) {
// update best number of inliers
bestNumInliers = currentInliers;
// keep inliers data corresponding to best number of
// inliers
bestNumberInliersData = inliersData;
// recompute number of times the algorithm needs to be
// executed depending on current number of inliers to
// achieve with probability mConfidence that we have
// inliers and probability 1 - mConfidence that we have
// outliers
final var probSubsetAllInliers = Math.pow((double) bestNumInliers / (double) totalSamples,
subsetSize);
if (Math.abs(probSubsetAllInliers) < Double.MIN_VALUE || Double.isNaN(probSubsetAllInliers)) {
newNIters = Integer.MAX_VALUE;
} else {
final var logProbSomeOutliers = Math.log(1.0 - probSubsetAllInliers);
if (Math.abs(logProbSomeOutliers) < Double.MIN_VALUE || Double.isNaN(logProbSomeOutliers)) {
newNIters = Integer.MAX_VALUE;
} else {
newNIters = (int) Math.ceil(Math.abs(Math.log(1.0 - confidence) / logProbSomeOutliers));
}
}
if (newNIters < iters) {
iters = newNIters;
}
}
// reset inliers data if either residual or number of inliers
// improved
if (inliersData.isMedianResidualImproved()) {
// create new inliers data instance until a new best solution
// is found
inliersData = new MSACInliersData(totalSamples);
// update the best median residual on new instance so that
// only better solutions that are found later can update
// inliers data
inliersData.update(bestMedianResidual, inliersData.getInliers(), inliersData.getResiduals(),
inliersData.getNumInliers(), false);
}
}
if (iters > 0) {
progress = Math.min((float) currentIter / (float) iters, 1.0f);
} else {
progress = 1.0f;
}
if (progress - previousProgress > progressDelta) {
previousProgress = progress;
listener.onEstimateProgressChange(this, progress);
}
currentIter++;
listener.onEstimateNextIteration(this, currentIter);
}
// no solution could be found after completing all iterations
if (bestResult == null) {
throw new RobustEstimatorException();
}
listener.onEstimateEnd(this);
return bestResult;
} catch (final SubsetSelectorException e) {
throw new RobustEstimatorException(e);
} finally {
locked = false;
}
}
/**
* Returns data about inliers once estimation has been done.
*
* @return data about inliers or null if estimation has not been done.
*/
@Override
public InliersData getInliersData() {
return getBestNumberInliersData();
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Computes inliers data for current iteration.
*
* @param <T> type of result to be estimated.
* @param iterResult result to be tested on current iteration.
* @param threshold threshold to determine whether samples are inliers or
* not.
* @param residualsTemp temporal array to store residuals, since median
* computation requires modifying the original array.
* @param listener listener to obtain residuals for samples.
* @param sorter sorter instance to compute median of residuals.
* @param inliersData inliers data to be reused on each iteration
*/
private static <T> void computeInliers(
final T iterResult, final double threshold, final double[] residualsTemp,
final LMedSRobustEstimatorListener<T> listener, final Sorter<Double> sorter,
final MSACInliersData inliersData) {
final var residuals = inliersData.getResiduals();
final var inliers = inliersData.getInliers();
var bestMedianResidual = inliersData.getBestMedianResidual();
var medianResidualImproved = false;
final var totalSamples = residuals.length;
double residual;
var numInliers = 0;
// find residuals and inliers
for (var i = 0; i < totalSamples; i++) {
residual = Math.abs(listener.computeResidual(iterResult, i));
if (residual < threshold) {
residuals[i] = residual;
numInliers++;
inliers.set(i);
} else {
residuals[i] = threshold;
inliers.clear(i);
}
}
// compute median of residuals
System.arraycopy(residuals, 0, residualsTemp, 0, residuals.length);
final var medianResidual = sorter.median(residualsTemp);
if (medianResidual < bestMedianResidual) {
bestMedianResidual = medianResidual;
medianResidualImproved = true;
}
// store values in inliers data, only if residuals improve
if (medianResidualImproved) {
inliersData.update(bestMedianResidual, inliers, residuals, numInliers, true);
}
}
/**
* Contains data related to inliers estimated in one iteration.
*/
public static class MSACInliersData extends InliersData {
/**
* Best median of error found so far taking into account all provided
* samples.
*/
private double bestMedianResidual;
/**
* Efficiently stores which samples are considered inliers and which
* ones aren't.
*/
private BitSet inliers;
/**
* Indicates whether median residual computed in current iteration has
* improved respect to previous iterations.
*/
private boolean medianResidualImproved;
/**
* Constructor.
*
* @param totalSamples total number of samples.
*/
protected MSACInliersData(final int totalSamples) {
bestMedianResidual = Double.MAX_VALUE;
inliers = new BitSet(totalSamples);
residuals = new double[totalSamples];
numInliers = 0;
medianResidualImproved = false;
}
/**
* Returns best median of error found so far taking into account all
* provided samples.
*
* @return best median of error found so far taking into account all
* provided samples.
*/
public double getBestMedianResidual() {
return bestMedianResidual;
}
/**
* Returns efficient array indicating which samples are considered
* inliers and which ones aren't.
*
* @return array indicating which samples are considered inliers and
* which ones aren't.
*/
@Override
public BitSet getInliers() {
return inliers;
}
/**
* Returns boolean indicating whether median residual computed in
* current iteration has improved respect to previous iterations.
*
* @return true if median residual improved, false otherwise.
*/
public boolean isMedianResidualImproved() {
return medianResidualImproved;
}
/**
* Updates data contained in this instance.
*
* @param bestMedianResidual best median of error found so far taking
* into account all provided samples.
* @param inliers efficiently stores which samples are considered
* inliers and which ones aren't.
* @param residuals residuals obtained for each sample of data.
* @param numInliers number of inliers found on current iteration.
* @param medianResidualImproved indicates whether median residual
* computed in current iteration has improved respect to previous
* iteration.
*/
protected void update(final double bestMedianResidual, final BitSet inliers,
final double[] residuals, final int numInliers, final boolean medianResidualImproved) {
this.bestMedianResidual = bestMedianResidual;
this.inliers = inliers;
this.residuals = residuals;
this.numInliers = numInliers;
this.medianResidualImproved = medianResidualImproved;
}
}
}