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