
/* 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).
 */

/* ImageMatchModel.cs
 *
 * Two image align model for use with RANSAC filtering. Will provide a
 * viewport transformation from one image coordinates to another images, given
 * some keypoint matches between the images. Then, provides information about
 * the likely correctness of further matches.
 *
 * (C) Copyright 2004 -- Sebastian Nowozin (nowozin@cs.tu-berlin.de)
 */

using System;
using System.Collections;


public class ImageMatchModel : RANSAC.IRANSACModel
{
	// The two original matches we build the model on.
	Match m1, m2;

	double fitThresh;

	// The distance-gratifying factor in the distance relaxing formula.
	double distanceFactor;
	// The image resolution to calculate the maximum possible distance.
	int width, height;

	// ICloneable
	public object Clone ()
	{
		ImageMatchModel mod = new ImageMatchModel (fitThresh,
			distanceFactor, width, height);

		mod.trans = (AffineTransform2D) trans.Clone ();

		return (mod);
	}

	// IComparable
	public int CompareTo (object obj)
	{
		ImageMatchModel mod = (ImageMatchModel) obj;

		if (this.FittingErrorSum < mod.FittingErrorSum)
			return (-1);
		else if (this.FittingErrorSum > mod.FittingErrorSum)
			return (1);

		return (0);
	}

	// IRANSACModel
	public bool FitModel (ArrayList matches)
	{
		if (matches.Count < 2) {
			//Console.WriteLine ("ImageMatchModel.FitModel: Need at least two matches to fit.");

			return (false);
		}

		// TODO: least-square match if more than two points are given.
		// For now, just ignore any further matches.
		m1 = (Match) matches[0];
		m2 = (Match) matches[1];

		try {
			//Console.WriteLine ("Doing transform building...");
			trans = AffineTransform2D.BuildTransformFromTwoPairs
				(m1.Kp1.X, m1.Kp1.Y, m2.Kp1.X, m2.Kp1.Y,
					m1.Kp2.X, m1.Kp2.Y, m2.Kp2.X, m2.Kp2.Y,
					width / 2, height / 2);
			//Console.WriteLine ("   resulting angle: {0}", trans.CenterAngle);
		} catch (ArgumentException aEx) {
			/*Console.WriteLine ("AffineTransform2D.BuildTransformFromTwoPairs: {0}",
				aEx.ToString ());
			*/

			return (false);
		}

		return (true);
	}

	public double FittingErrorSingle (object match)
	{
		Match m = (Match) match;

		// Build homegenous coordinates for the point in the second (right)
		// image.
		SimpleMatrix X = new SimpleMatrix (3, 1);
		X[0, 0] = m.Kp2.X;
		X[1, 0] = m.Kp2.Y;
		X[2, 0] = 1.0;

		// Calculate the points expected position in the first (left) image.
		SimpleMatrix Xexpected = trans * X;

		// Now calculate the difference/distance between the expected and the
		// real point position.
		/*Console.WriteLine ("exp, real: ({0} ; {1}) - ({2} ; {3}) # RANSACREALEXP",
			Xexpected[0, 0], Xexpected[1, 0], m.Kp1.X, m.Kp1.Y);*/
		double pixDistance = Math.Sqrt (Math.Pow (m.Kp1.X - Xexpected[0, 0], 2.0) +
			Math.Pow (m.Kp1.Y - Xexpected[1, 0], 2.0));

		// Now, we cheat a little. As we cannot associate information with the
		// value we return, but want to adjust for distance based skews in the
		// model, we will subtract the pixel distance with a distance term. If
		// we go below zero, we restore it to zero.
		// The term to subtract is, in LaTeX notation:
		// d_{f} \cdot \left(\frac{d (A, B, X)}{\sqrt{w^2 + h^2}}\right) \cdot t
		//
		// Where d_{f} is the distance factor, d the triangular distance
		// function and t the fitting threshhold. The d(A,B,X) function is
		// defined as:
		//
		// d (A, B, X) := |X - A| + |X - B| - |A - B|
		//
		// and ensures a smooth eliptical weighting around what we assume to
		// be good model predictions.
		double distXA = Math.Sqrt (Math.Pow (m.Kp2.X - m1.Kp2.X, 2) +
			Math.Pow (m.Kp2.Y - m1.Kp2.Y, 2));
		double distXB = Math.Sqrt (Math.Pow (m.Kp2.X - m2.Kp2.X, 2) +
			Math.Pow (m.Kp2.Y - m2.Kp2.Y, 2));
		double distAB = Math.Sqrt (Math.Pow (m1.Kp2.X - m2.Kp2.X, 2) +
			Math.Pow (m1.Kp2.Y - m2.Kp2.Y, 2));

		double distanceTerm = distXA + distXB - distAB;
		distanceTerm /= Math.Sqrt (Math.Pow (width, 2) + Math.Pow (height, 2));
		double distanceReduce = distanceFactor * distanceTerm * fitThresh;
		/*Console.WriteLine ("reducing distance {0} by {1} due to distance term {2}, thresh {3}",
			pixDistance, distanceReduce, distanceTerm, fitThresh);*/
		pixDistance -= distanceReduce;

		if (pixDistance < 0.0)
			pixDistance = 0.0;

		return (pixDistance);
	}

	public bool ThreshholdPoint (double fitError)
	{
		/*Console.WriteLine ("TreshholdPoint: {0} to {1} # RANSACTHRESH",
			fitError, fitThresh);*/
		if (fitError > fitThresh)
			return (false);

		return (true);
	}

	private double fittingErrorSum = 0.0;
	public double FittingErrorSum {
		get {
			return (fittingErrorSum);
		}
		set {
			fittingErrorSum = value;
		}
	}

	private ArrayList fittingGround = null;
	public ArrayList FittingGround {
		get {
			return (fittingGround);
		}
		set {
			fittingGround = value;
		}
	}

	public int ShiftWidth {
		get {
			return (trans.ShiftWidth);
		}
	}

	public double RotationAngle {
		get {
			return (trans.RotationAngle);
		}
	}

	public double CenterAngle {
		get {
			return (trans.CenterAngle);
		}
	}

	// Internals
	private AffineTransform2D trans = null;

	// Constructor
	// fitThresh: Threshhold value for further matching of a single match. If
	// the position of the transformed pixel is more than fitThresh pixels
	// away, its considered invalid.
	// distanceFactor: The distance gratifying factor. Use zero to nullify the
	// effect of distance based relaxing.
	// width, heigth: Image format.
	public ImageMatchModel (double fitThresh, double distanceFactor,
		int width, int height)
	{
		this.fitThresh = fitThresh;
		this.distanceFactor = distanceFactor;
		this.width = width;
		this.height = height;
	}

	public override string ToString ()
	{
		return (String.Format ("Model: sumError = {0}", fittingErrorSum));
	}
}


// Driver class for the RANSAC processing.

public class MatchDriver
{
	public static ImageMatchModel FilterMatchSet (ArrayList matches,
		double distanceFactor, int width, int height)
	{
		// Need at least one match pair plus one for verification to use
		// RANSAC. If we only have two we could apply RANSAC, but it would
		// always fit perfectly, so its of no use.
		if (matches.Count <= 2)
			return (null);

		// Create a generic RANSAC algorithm processor, which requires at
		// least two points to be able to fit the model, with an expected 50%
		// correct matchrate and an additional safe-margin of five times the
		// standard-deviation of the correct match probabilities.
		RANSAC ran = new RANSAC (2,
			RANSAC.GetKFromGoodfraction (2, 0.5, 10));

		// Create a template model with a threshhold distance of 10 pixels.
		ImageMatchModel mod = new ImageMatchModel
			(4.0, distanceFactor, width, height);

		// Find appropiate models with two additional matching points
		// required. (FIXME: Maybe change to 1?)
		ArrayList models = ran.FindModels (mod, matches, 2);
		/*foreach (ImageMatchModel model in models)
			Console.WriteLine (model.ToString ());*/

		if (models.Count == 0)
			return (null);

		ImageMatchModel best = (ImageMatchModel) models[0];

		return (best);
	}
}


