Decompose Homography into Rotation matrix & Translation vector

HomographyDecomposition.as

var intrinsic:Vector.<Number> = new Vector.<Number>(9, true);
var intrinsicInverse:Vector.<Number> = new Vector.<Number>(9, true);
		
var R:Vector.<Number> = new Vector.<Number>( 9, true );
var t:Vector.<Number> = new Vector.<Number>( 3, true );

// SVD routine
var svd:SVD = new SVD();

// input homography[9] - 3x3 Matrix
// please note that homography should be computed
// using centered object/reference points coordinates
// for example coords from [0, 0], [320, 0], [320, 240], [0, 240]
// should be converted to [-160, -120], [160, -120], [160, 120], [-160, 120]
function computePose(homography:Vector.<Number>):Boolean
{
	var h10:Number = homography[0];
	var h11:Number = homography[3];
	var h12:Number = homography[6];
	var h20:Number = homography[1];
	var h21:Number = homography[4];
	var h22:Number = homography[7];
	var h30:Number = homography[2];
	var h31:Number = homography[5];
	var h32:Number = homography[8];
	var r10:Number, r11:Number, r12:Number;
	var r20:Number, r21:Number, r22:Number;
	var r30:Number, r31:Number, r32:Number;
	var vT:Vector.<Number> = new Vector.<Number>(9, true);
	//
	var invC0:Number = intrinsicInverse[0];
	var invC1:Number = intrinsicInverse[1];
	var invC2:Number = intrinsicInverse[2];
	var invC3:Number = intrinsicInverse[3];
	var invC4:Number = intrinsicInverse[4];
	var invC5:Number = intrinsicInverse[5];
	var invC6:Number = intrinsicInverse[6];
	var invC7:Number = intrinsicInverse[7];
	var invC8:Number = intrinsicInverse[8];
	//
	var invH10:Number = invC0*h10 + invC1*h11 + invC2*h12;
	var invH11:Number = invC3*h10 + invC4*h11 + invC5*h12;
	var invH12:Number = invC6*h10 + invC7*h11 + invC8*h12;

	var lambda:Number = Math.sqrt( invH10 * invH10 + invH11 * invH11 + invH12 * invH12 );

	if (lambda == 0) return false;

	lambda = 1.0 / lambda;
	invC0 *= lambda;
	invC1 *= lambda;
	invC2 *= lambda;
	invC3 *= lambda;
	invC4 *= lambda;
	invC5 *= lambda;
	invC6 *= lambda;
	invC7 *= lambda;
	invC8 *= lambda;

	// Create normalized R1 & R2:
	r10 = invC0*h10 + invC1*h11 + invC2*h12;
	r11 = invC3*h10 + invC4*h11 + invC5*h12;
	r12 = invC6*h10 + invC7*h11 + invC8*h12;
	//
	r20 = invC0*h20 + invC1*h21 + invC2*h22;
	r21 = invC3*h20 + invC4*h21 + invC5*h22;
	r22 = invC6*h20 + invC7*h21 + invC8*h22;

	// Get R3 orthonormal to R1 and R2:
	r30 = r11 * r22 - r12 * r21;
	r31 = r12 * r20 - r10 * r22;
	r32 = r10 * r21 - r11 * r20;

	// Put the rotation column vectors in the rotation matrix:
	R[0] = r10;
	R[1] = r20;
	R[2] = r30;
	R[3] = r11;
	R[4] = r21;
	R[5] = r31;
	R[6] = r12;
	R[7] = r22;
	R[8] = r32;

	// Calculate Translation Vector T:
	t[0] = invC0*h30 + invC1*h31 + invC2*h32;
	t[1] = invC3*h30 + invC4*h31 + invC5*h32;
	t[2] = invC6*h30 + invC7*h31 + invC8*h32;
	
	// Transformation of R into - in Frobenius sense - next orthonormal matrix:
	svd.decompose( R, 3, 3 );
	transposeMat( svd.V, vT );
	multMat( svd.U, vT, R );
	
	return true;
}

function setIntrinsicParams(fx:Number, fy:Number, cx:Number, cy:Number):void
{
	intrinsic[0] = fx;
	intrinsic[4] = fy;
	intrinsic[2] = cx;
	intrinsic[5] = cy;
	intrinsic[8] = 1.0;
	//
	// Create inverse calibration matrix:
	var tau:Number = fx / fy;
	intrinsicInverse[0] = 1.0 / (tau*fy);
	intrinsicInverse[1] = 0.0;
	intrinsicInverse[2] = -cx / (tau*fy);
	intrinsicInverse[3] = 0.0;
	intrinsicInverse[4] = 1.0 / fy;
	intrinsicInverse[5] = -cy / fy;
	intrinsicInverse[6] = 0.0;
	intrinsicInverse[7] = 0.0;
	intrinsicInverse[8] = 1.0;
}