Path: news.mathworks.com!not-for-mail
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$1@fred.mathworks.com>
References: <glcep2$jj1$1@fred.mathworks.com>
Reply-To: <HIDDEN>
NNTP-Posting-Host: webapp-02-blr.mathworks.com
Content-Type: text/plain; charset="ISO-8859-1"
Content-Transfer-Encoding: 8bit
X-Trace: fred.mathworks.com 1232763962 20505 172.30.248.37 (24 Jan 2009 02:26:02 GMT)
X-Complaints-To: news@mathworks.com
NNTP-Posting-Date: Sat, 24 Jan 2009 02:26:02 +0000 (UTC)
X-Newsreader: MATLAB Central Newsreader 1187260
Xref: news.mathworks.com comp.soft-sys.matlab:513593

"Geoffrey" <geoffrey.bourque@ch.abb.com> wrote in message <glcep2$jj1$1@fred.mathworks.com>...
> 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 (http://planning.cs.uiuc.edu/node104.html). 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: http://planning.cs.uiuc.edu/node103.html 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 

 http://www.mathworks.com/matlabcentral/newsreader/view_thread/169096

  In order to then find the angles you seek, you can either use the method you referenced at http://planning.cs.uiuc.edu/node103.html or you can use a method discussed in the thread you found, "angle from rotation matrix", at 

 http://www.mathworks.com/matlabcentral/newsreader/view_thread/160945

  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