// Copyright (c) 2007, 2008 libmv authors. // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to // deal in the Software without restriction, including without limitation the // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or // sell copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: // // The above copyright notice and this permission notice shall be included in // all copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. #ifndef LIBMV_MULTIVIEW_ROBUST_ESTIMATION_H_ #define LIBMV_MULTIVIEW_ROBUST_ESTIMATION_H_ #include <set> #include "libmv/base/vector.h" #include "libmv/logging/logging.h" #include "libmv/multiview/random_sample.h" #include "libmv/numeric/numeric.h" namespace libmv { template<typename Kernel> class MLEScorer { public: MLEScorer(double threshold) : threshold_(threshold) {} double Score(const Kernel &kernel, const typename Kernel::Model &model, const vector<int> &samples, vector<int> *inliers) const { double cost = 0.0; for (int j = 0; j < samples.size(); ++j) { double error = kernel.Error(samples[j], model); if (error < threshold_) { cost += error; inliers->push_back(samples[j]); } else { cost += threshold_; } } return cost; } private: double threshold_; }; static uint IterationsRequired(int min_samples, double outliers_probability, double inlier_ratio) { return static_cast<uint>( log(outliers_probability) / log(1.0 - pow(inlier_ratio, min_samples))); } // 1. The model. // 2. The minimum number of samples needed to fit. // 3. A way to convert samples to a model. // 4. A way to convert samples and a model to an error. // // 1. Kernel::Model // 2. Kernel::MINIMUM_SAMPLES // 3. Kernel::Fit(vector<int>, vector<Kernel::Model> *) // 4. Kernel::Error(Model, int) -> error template<typename Kernel, typename Scorer> typename Kernel::Model Estimate(const Kernel &kernel, const Scorer &scorer, vector<int> *best_inliers = NULL, double *best_score = NULL, double outliers_probability = 1e-2) { CHECK(outliers_probability < 1.0); CHECK(outliers_probability > 0.0); size_t iteration = 0; const size_t min_samples = Kernel::MINIMUM_SAMPLES; const size_t total_samples = kernel.NumSamples(); size_t max_iterations = 100; const size_t really_max_iterations = 1000; int best_num_inliers = 0; double best_cost = HUGE_VAL; double best_inlier_ratio = 0.0; typename Kernel::Model best_model; // Test if we have sufficient points to for the kernel. if (total_samples < min_samples) { if (best_inliers) { best_inliers->resize(0); } return best_model; } // In this robust estimator, the scorer always works on all the data points // at once. So precompute the list ahead of time. vector<int> all_samples; for (int i = 0; i < total_samples; ++i) { all_samples.push_back(i); } vector<int> sample; for (iteration = 0; iteration < max_iterations && iteration < really_max_iterations; ++iteration) { UniformSample(min_samples, total_samples, &sample); vector<typename Kernel::Model> models; kernel.Fit(sample, &models); VLOG(4) << "Fitted subset; found " << models.size() << " model(s)."; // Compute costs for each fit. for (int i = 0; i < models.size(); ++i) { vector<int> inliers; double cost = scorer.Score(kernel, models[i], all_samples, &inliers); VLOG(5) << "Fit cost: " << cost << ", number of inliers: " << inliers.size(); if (cost < best_cost) { best_cost = cost; best_inlier_ratio = inliers.size() / double(total_samples); best_num_inliers = inliers.size(); best_model = models[i]; if (best_inliers) { best_inliers->swap(inliers); } VLOG(4) << "New best cost: " << best_cost << " with " << best_num_inliers << " inlying of " << total_samples << " total samples."; } if (best_inlier_ratio) { max_iterations = IterationsRequired(min_samples, outliers_probability, best_inlier_ratio); } VLOG(5) << "Max iterations needed given best inlier ratio: " << max_iterations << "; best inlier ratio: " << best_inlier_ratio; } } if (best_score) *best_score = best_cost; return best_model; } } // namespace libmv #endif // LIBMV_MULTIVIEW_ROBUST_ESTIMATION_H_