Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Decompose Homography into Rotation matrix & Translation vector
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;
}
@stephanepechard

This comment has been minimized.

Copy link

@stephanepechard stephanepechard commented Mar 5, 2012

Hi,
I don't get your multMat operation (I don't know ActionScript), is it:

R = U * vT

or something else?
Thanks

@inspirit

This comment has been minimized.

Copy link
Owner Author

@inspirit inspirit commented Mar 18, 2012

yeah it is just simple matrix multiplication.
the latest source and all methods available at my google code: http://code.google.com/p/in-spirit/

@greydon

This comment has been minimized.

Copy link

@greydon greydon commented Aug 20, 2013

Hi,can i compute homography using rotation and translation?

@Ziyou1987

This comment has been minimized.

Copy link

@Ziyou1987 Ziyou1987 commented Sep 8, 2014

Hi, do you know how to decompose the homography matrix to get the surface normal ? Thanks

@wutangyibo

This comment has been minimized.

Copy link

@wutangyibo wutangyibo commented Oct 29, 2017

Hi, I don't understand principle about the code. Is there any document for the code.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment