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 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 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 commented Aug 20, 2013

Hi,can i compute homography using rotation and translation?

@Ziyou1987

This comment has been minimized.

Copy link

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 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
You can’t perform that action at this time.