Result solve(const Problem& iProblem) const {
// set up initial (empty) result
Result result;
result.mSuccess = false;
result.mNumIterations = 0;
// ensure that there are enough data points to proceed
const int sampleSize = iProblem.getSampleSize();
const int n = iProblem.getNumDataPoints();
if (n < sampleSize) {
return result;
}
const double epsilon = 1e-10;
// best results are currently invalid
double bestScore = -1;
bool success = false;
// start number of iterations as infinite, then reduce as we go
double numIterationsNeeded = 1e10;
int iterationCount = 0;
int skippedSampleCount = 0;
// for random sample index generation
std::vector<int> allIndices(n);
// iterate until adaptive number of iterations are exceeded
while (iterationCount < numIterationsNeeded) {
// determine random sample indices
for (int i = 0; i < n; ++i) {
allIndices[i] = i;
}
for (int i = 0; i < sampleSize; ++i) {
int randIndex = std::rand() % n;
std::swap(allIndices[i], allIndices[randIndex]);
}
std::vector<int> sampleIndices(allIndices.begin(),
allIndices.begin() + sampleSize);
// compute solution on minimal set
typename Problem::Solution solution = iProblem.estimate(sampleIndices);
// compute errors over all data points
std::vector<double> errors2 = iProblem.computeSquaredErrors(solution);
// check whether this is a valid sample
// TODO: this should be done via a method in Problem class, but would
// require changing all existing usages to include that method
if (errors2.size() == 0) {
++skippedSampleCount;
if (skippedSampleCount >= mMaximumIterations) break;
continue;
}
skippedSampleCount = 0;
// compute error threshold to be applied to each term
double thresh = mMaximumError;
if (thresh < 0) {
std::sort(errors2.begin(), errors2.end());
double median = (n % 2 == 0) ?
(0.5*(errors2[n/2]+errors2[n/2+1])) : errors2[n/2];
thresh = 1.4826*std::sqrt(median)*4.6851;
}
thresh *= thresh;
// determine inliers
std::vector<int> inliers;
inliers.reserve(n);
for (int i = 0; i < n; ++i) {
if (errors2[i] <= thresh) {
inliers.push_back(i);
}
}
// if this is the best score, update solution and convergence criteria
double score = inliers.size();
if (score > bestScore) {
bestScore = score;
result.mInliers = inliers;
result.mSolution = solution;
success = true;
double inlierProbability = double(inliers.size()) / n;
double anyOutlierProbability = 1 - pow(inlierProbability,sampleSize);
anyOutlierProbability = std::min(anyOutlierProbability, 1-epsilon);
anyOutlierProbability = std::max(anyOutlierProbability, epsilon);
numIterationsNeeded =
log(1-mGoodSolutionProbability) / log(anyOutlierProbability);
}
// bump up iteration count and terminate if it exceeds hard max
++iterationCount;
if (iterationCount > mMaximumIterations) {
break;
}
}
// finish off result params
result.mSuccess = success;
result.mNumIterations = iterationCount;
// refine result using all inliers if specified
if (result.mSuccess && mRefineUsingInliers) {
result.mSolution = iProblem.estimate(result.mInliers);
}
// done
return result;
}
