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; }