www.pudn.com > bsiftC_.rar > Transform.cs

/* Transform.cs
 *
 * Simple 2d transformation functions.
 *
 * (C) Copyright 2004 -- Sebastian Nowozin (nowozin@cs.tu-berlin.de)
 */

using System;

public class AffineTransform2D : SimpleMatrix, ICloneable
{
	public override object Clone ()
	{
		AffineTransform2D cl = new AffineTransform2D ();

		for (int y = 0 ; y < YDim ; ++y)
			for (int x = 0 ; x < XDim ; ++x)
				cl[y, x] = this[y, x];

		return (cl);
	}

	public AffineTransform2D ()
		: base (3, 3)
	{
	}

	// The shift length in pixels.
	public int ShiftWidth {
		get {
			SimpleMatrix X = new SimpleMatrix (3, 1);
			X[0, 0] = 0.0;
			X[1, 0] = 0.0;
			X[2, 0] = 1.0;

			SimpleMatrix xI1 = this * X;
			/*Console.WriteLine ("xI1[0,0] = {0}, xI1[1,0] = {1}",
				xI1[0, 0], xI1[1, 0]);*/

			double length = Math.Sqrt (Math.Pow (xI1[0, 0], 2.0) +
				Math.Pow (xI1[1, 0], 2.0));
			//Console.WriteLine ("length = {0}", length);

			return ((int) length);
		}
	}

	// The relative angle of both images in the affine transformation.
	double rotationAngle = -98.0;
	public double RotationAngle {
		get {
			return (rotationAngle);
		}
	}

	// The angle between the horizon and the line through the centers of both
	// images.
	double centerAngle = -99; // FIXME: remove this dummy value for debug purposes
	public double CenterAngle {
		get {
			return (centerAngle);
		}
	}

	// Calculate a new transformation from two matching points in two views.
	// That is, one view (lets call it Image I1) has two points
	// A1 = (a1x, a1y), B1 = (b1x, b1y). Those points should anchor a
	// translated, rotated and scaled coordinate system so that they transform
	// affinely to A2 = (a2x, a2y), B2 = (b2x, b2y) in I2.
	//
	// The transformation matrix build is homogeneous 3x3:
	//   s * cos(T)   s * (-sin(T))  s * (cos(T)*(-a2x) - sin(T)*(-a2y)) + a1x
	//   s * sin(T)   s * cos(T)     s * (sin(T)*(-a2x) + cos(T)*(-a2y)) + a1y
	//   0            0              1
	// where
	//   s is the scale factor with
	//      s = |(a1x ; a1y)| / |(a2x ; a2y)|
	//   T is the rotation angle with
	//      T = atan2 (b1y - a1y ; b1x - a1x) - atan2 (b2y - a2y ; b2x - a2x)
	//
	// The matrix is combined from T_2 * S * R * T_1, where T_1 transforms the
	// points in I2 to the origin, R rotates it into the direction of the line
	// A1-B1, S scales it so the length of A1-B1 and A2-B2 match and T_2
	// transforms it relative to A1.
	//
	// The final transform is a mapping from points of I2 into I1.
	public static AffineTransform2D BuildTransformFromTwoPairs (
		double a1x, double a1y, double b1x, double b1y,
		double a2x, double a2y, double b2x, double b2y,
		int centerX, int centerY)
	{
		AffineTransform2D trans = new AffineTransform2D ();

		/*Console.WriteLine ("Build from: A1({0};{1})-B1({2};{3}) to A2({4};{5})-B2({6};{7})",
			a1x, a1y, b1x, b1y, a2x, a2y, b2x, b2y);*/

		// Calculate the rotation angle for later use.
		double angle = trans.rotationAngle =
			Math.Atan2 (b1y - a1y, b1x - a1x) - Math.Atan2 (b2y - a2y, b2x - a2x);
		double sinAngle = Math.Sin (angle);
		double cosAngle = Math.Cos (angle);

		// Calculate the scale for later use.
		double s = Math.Sqrt ((b2x - a2x)*(b2x - a2x) + (b2y - a2y)*(b2y - a2y));
		if (s == 0.0)
			throw (new ArgumentException
				("A2 is identical to B2, zero length. Cannot continue."));
		s = Math.Sqrt ((b1x - a1x)*(b1x - a1x) + (b1y - a1y)*(b1y - a1y)) / s;

		trans[0, 0] = s * cosAngle;
		trans[0, 1] = s * -sinAngle;
		trans[0, 2] = s * (cosAngle * (-a2x) - sinAngle * (-a2y)) + a1x;
		trans[1, 0] = s * sinAngle;
		trans[1, 1] = s * cosAngle;
		trans[1, 2] = s * (sinAngle * (-a2x) + cosAngle * (-a2y)) + a1y;
		trans[2, 0] = 0.0;
		trans[2, 1] = 0.0;
		trans[2, 2] = 1.0;

		// Calculate the center-center angle from center-east to center-east
		// of both images
		SimpleMatrix X = new SimpleMatrix (3, 1);
		X[0, 0] = centerX;
		X[1, 0] = centerY;
		X[2, 0] = 1.0;
		SimpleMatrix xI1 = trans * X;
		trans.centerAngle = Math.Atan2 (xI1[1, 0] - centerY,
			xI1[0, 0] - centerX);
		/*
		Console.WriteLine ("  C ({0},{1}) -> ({2},{3}) -> ({4},{5}) => {6}",
			centerX, centerY, xI1[0, 0], xI1[1, 0],
			xI1[0, 0] - centerX, xI1[1, 0] - centerY, trans.CenterAngle);
		*/

		return (trans);
	}

	public static void Main (string[] args)
	{
		AffineTransform2D M = AffineTransform2D.BuildTransformFromTwoPairs
			(1.0, 2.0, 4.0, 2.0, 3.0, 2.0, 6.0, 4.0, 320, 240);

		Console.WriteLine ("M = {0}", M.ToString ());

		SimpleMatrix X = new SimpleMatrix (3, 1);
		X[0, 0] = 4;
		X[1, 0] = 5;
		X[2, 0] = 1;

		Console.WriteLine ("M * X = {0}", M * X);
	}
}