From: <HIDDEN>
Newsgroups: comp.soft-sys.matlab
Subject: Re: Homogeneous transform (angles required, matrix not given)
Date: Sat, 24 Jan 2009 02:26:02 +0000 (UTC)
Organization: The MathWorks, Inc.
Lines: 46
Message-ID: <gldu7q$k0p$>
References: <glcep2$jj1$>
Reply-To: <HIDDEN>
Content-Type: text/plain; charset="ISO-8859-1"
Content-Transfer-Encoding: 8bit
X-Trace: 1232763962 20505 (24 Jan 2009 02:26:02 GMT)
NNTP-Posting-Date: Sat, 24 Jan 2009 02:26:02 +0000 (UTC)
X-Newsreader: MATLAB Central Newsreader 1187260
Xref: comp.soft-sys.matlab:513593

"Geoffrey" <> wrote in message <glcep2$jj1$>...
> I've already checked through a post titled: "angle from rotation matrix", but it does not seem to apply to my case.
> I've got a problem - it involves finding the angles and displacements inside of a general homogeneous transform matrix ( That is indeed the form of the matrix T that I am given. In general there are then 6 unknowns in the matrix.
> I have two matrix equations, Ta1 = b1, Ta2 = b2.  So a1, a2, b1, b2 are all given. They're 4x1 vectors, and in general the systems should have 8 equations. But two equations are actually useless (the result of the last row crossed with the vectors yields 1=1 for both pairs of (a,b)). So I'm left with 6 equations, and 6 unknowns. 
> In principle I should be able to solve this. Taking the advice from the page: I can try and find the pitch and roll. But when I write out the equations I actually have 12 unknowns and 6 equations - so I can't really ever isolate nicely. 
> So what it seems is that I have a pretty sweet non-linear problem. Any help would be much appreciated. (And thanks in advance for those who do.)

  Matt is quite right, Geoffrey.  You need three pairs of points to determine your T matrix uniquely and these must not be colinear. There must be a third equation, T*a3=b3.  Moreover, not just any three pairs will do.  The triangle formed by a1, a2, and a3 must be congruent to the triangle formed by b1, b2, and b3.

  I will give you a matlab procedure for finding T without explanation.  If you need to understand the logic behind this, you can consult the literature concerning the "Procrustes" problem, or you can look up an explanation I gave in the thread "Procrustes Analysis without Reflection" at

  In order to then find the angles you seek, you can either use the method you referenced at or you can use a method discussed in the thread you found, "angle from rotation matrix", at

  In accordance with existing convention I assume that the a and b vectors are each 4x1 with a one in the bottom place.  Also I assume T is 4x4 with a 3x3 rotation (unitary) matrix in the upper left corner, a 3x1 displacement vector in the upper right corner and [0 0 0 1] in the bottom row.

  For our purposes define X = [a1,a2,a3] and Y = [b1,b2,b3] (which will then be 4x3 arrays.)  The first part of your problem is this: given X and Y, determine T so that T*X=Y with T a valid rotation/translation matrix.  The matlab code would proceed as follows:

 x = X(1:3,:); y = Y(1:3,:); % Remove the bottom rows of ones
 xm = mean(x,2); ym = mean(y,2); % Get mean points
 xx = x - repmat(xm,1,3); % Translate x and y so mean points 
 yy = y - repmat(ym,1,3); % lie at the origin

 [U,S,V] = svd(yy*xx'); % Now find the purely rotation matrix
 P = U*V';
 if det(P) < 0, V(:,3) = -V(:,3); end
 P = U*V'; % P is now the desired 3x3 rotation matrix

 d = ym-P*xm; % Set up the displacement part
 T = [P,d;0 0 0 1]; % Finally assemble T

  The oddity above of conditionally altering the sign of the third column of V is due to the fact that the third (smallest) singular value in S will be zero if your a and b are valid sets of points, and therefore there is an indeterminacy as to the sign of the corresponding eigenvector.  A valid rotation matrix must have a determinant of +1, so if a -1 occurs, the third column of V is reversed in sign.

  If the above proves to be too mysterious and the given references too difficult to decipher, I will relent and attempt to give a justification for these steps in this thread.  (It might take me a few days to gather my thoughts, however.)

Roger Stafford