
/* autopano-sift, Automatic panorama image creation
 * Copyright (C) 2004 -- Sebastian Nowozin
 *
 * This program is free software released under the GNU General Public
 * License, which is included in this software package (doc/LICENSE).
 */

/* RANSAC - RANdom SAmple Consensus
 *
 * Generic RANSAC fitting functionality.
 *
 * (C) Copyright 2004 -- Sebastian Nowozin (nowozin@cs.tu-berlin.de)
 *
 * Based on "Computer Vision - a modern approach", Forsyth & Ponce, pp. 346
 */

using System;
using System.Collections;

public class RANSAC
{
	public interface IRANSACModel : ICloneable, IComparable
	{
		// Fit the model to the samples given. The number of samples is equal
		// to or larger than the smallest number of points required for a fit
		// ('n').
		// Return true if the fit can be done, false otherwise.
		bool FitModel (ArrayList points);

		// Return the fitting error of a single point against the current
		// model.
		double FittingErrorSingle (object point);

		// Threshhold the given fit error of a point.
		// Return true if the fitting error is small enough and the point is
		//     fitting.
		// Return false if the point is not fitting.
		bool ThreshholdPoint (double fitError);

		// The overall fitting error of all points in FittingGround. This
		// value is calculated by averaging all individual fitting errors of
		// the points in the FittingGround.
		double FittingErrorSum {
			get;
			set;
		}

		// All the points used to fit. Has to be set explicitly.
		ArrayList FittingGround {
			get;
			set;
		}
	}

	// Smallest number of points to be able to fit the model.
	private int n;

	// The number of iterations required.
	private int k;

	private RANSAC ()
	{
	}

	// n: Smallest number of points to be able to fit the model.
	// k: The number of iterations required.
	public RANSAC (int n, int k)
	{
		this.n = n;
		this.k = k;
	}

	// ArrayList of Model's, sorted by summed fitting error.
	// model: Model to fit
	// points: List of point data to fit
	// d: Number of nearby points required for a model to be accepted
	public ArrayList FindModels (IRANSACModel model, ArrayList points, int d)
	{
		Random rand = new Random ();
		ArrayList result = new ArrayList ();

		if (points.Count < n)
			throw (new ArgumentException
				("List of data is smaller than minimum fit requires."));

		for (int ki = 0 ; ki < k ; ++ki) {
			ArrayList samples = new ArrayList ();

			// Build random samples
			for (int ri = 0 ; ri < n ; ++ri) {
				object sampleToAdd;
				sampleToAdd = points[rand.Next (0, points.Count)];

				if (samples.Contains (sampleToAdd))
					continue;

				samples.Add (sampleToAdd);
			}

			if (model.FitModel (samples) == false)
				continue;

			ArrayList good = new ArrayList ();
			double overAllFittingError = 0.0;

			// Check all non-sample points for fit.
			foreach (object point in points) {
				if (samples.Contains (point))
					continue;

				double fitError = model.FittingErrorSingle (point);
				if (model.ThreshholdPoint (fitError)) {
					good.Add (point);
					overAllFittingError += fitError;
				}
			}

			// good contains a list of all fitting points now. Check if there
			// are more than d points near our model.
			if (good.Count >= d) {
				good.AddRange (samples);
				IRANSACModel modelGood = (IRANSACModel) model.Clone ();

				if (modelGood.FitModel (good) == false) {
					// This is a rare case when the distance between the
					// sample points is zero. It could happen, but is very
					// rare.
					//Console.WriteLine ("RANSAC: Fitting model from good samples failed, discarding this model.");
					continue;
				}

				modelGood.FittingErrorSum = overAllFittingError / good.Count;
				modelGood.FittingGround = good;

				result.Add (modelGood);
			}
		}
		result.Sort ();
		//Console.WriteLine ("got {0} modelfits", result.Count);

		return (result);
	}

	// Calculate the expected number of draws required when a fraction of
	// 'goodFraction' of the sample points is good and at least 'n' points are
	// required to fit the model. Add 'sdM' times the standard deviation to be
	// sure.
	// n: > 0
	// goodFraction: > 0.0 and <= 1.0
	// sdM: >= 0
	// return the guess for k, the expected number of draws.
	public static int GetKFromGoodfraction (int n, double goodFraction, int sdM)
	{
		double result;

		result = Math.Pow (goodFraction, -n);
		if (sdM > 0)
			result += sdM * Math.Sqrt (1.0 - Math.Pow (goodFraction, n));

		return ((int) (result + 0.5));
	}

	// Test Main
	public static void Main (string[] args)
	{
		Console.WriteLine ("n = 3, goodFraction = 0.3, sdM = 0: {0}",
			GetKFromGoodfraction (3, 0.3, 0));
		Console.WriteLine ("n = 3, goodFraction = 0.3, sdM = 10: {0}",
			GetKFromGoodfraction (3, 0.3, 10));
	}
}

