Thread Subject:

Homogeneous transform (angles required, matrix not given)

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.)

"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.)

You need a 3rd equation T*a3=b3 to solve this.

"Matt " <mjacobson.removethis@xorantech.com> wrote in message <glcpnn$87h$1@fred.mathworks.com>...
> "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.)
>
> You need a 3rd equation T*a3=b3 to solve this.
>


Too brief?

To see why you need an additional equation, imagine that T is a solution to Ta1 = b1, Ta2 = b2.

Now, suppose I make the modification Tnew=T*Trot
where Trot performs a rotation by an arbitrary angle around the axis running through a1 and a2. In other words Trot*a1=a1 and Trot*a2=a2 will not change a1 and a2 at all.

This means Tnew must also solve your original equations. So, you have an under-determined system...

"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

"Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <gldu7q$k0p$1@fred.mathworks.com>...
> .......
> 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.
> ......

  My explanation of dealing with the case when det(P) = -1 was not entirely clear. I should have added something like this.

  For a set of only the three vertices of congruent triangles to be matched up, it is always possible to find two solutions for T that will do an exact match, one of them a pure rotation/translation and the other a reflection/rotation/translation, with the determinants of the P's possessing opposite signs. It is in the nature of triangles in three dimensions that they can always be rotated into a mirror image of themselves. For four or more points to be matched up, this is in general not possible. (Envision the left- and right-handed stereo-isomers in chemistry.)

  It should be pointed out that the method I gave can be extended to more than three points and to sets X and Y that can't be matched exactly. The same procedure will give the nearest possible match in the least squares sense. It is a special case of the general "Procrustes" problem which also provides for rescaling.

Roger Stafford

I would also just point out the following reference.

http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf

It gives the least squares solution to T, which is probably wise to use since your measured data pointsa1...a3, b1...b3 are unlikely to be exact rigid transformatins of each other

After you've found T, you can sift out the angles as Roger has described.

"Matt " <mjacobson.removethis@xorantech.com> wrote in message <glfnfm$3eu$1@fred.mathworks.com>...
> I would also just point out the following reference.
>
> http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf
>
> It gives the least squares solution to T, which is probably wise to use since your measured data pointsa1...a3, b1...b3 are unlikely to be exact rigid transformatins of each other
>
> After you've found T, you can sift out the angles as Roger has described.

  Actually what I gave is a least squares solution, Matt. If the two triples of points don't exactly match, it automatically gets a solution with the least squares error.

Roger Stafford

"Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <glfqmp$267$1@fred.mathworks.com>...
> "Matt " <mjacobson.removethis@xorantech.com> wrote in message <glfnfm$3eu$1@fred.mathworks.com>...
> > I would also just point out the following reference.
> >
> > http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf
> >
> > It gives the least squares solution to T, which is probably wise to use since your measured data pointsa1...a3, b1...b3 are unlikely to be exact rigid transformatins of each other
> >
> > After you've found T, you can sift out the angles as Roger has described.
>
> Actually what I gave is a least squares solution, Matt. If the two triples of points don't exactly match, it automatically gets a solution with the least squares error.
>
> Roger Stafford

Yes, Roger, I see that now. A comparison between your proposal and mine (and 2 others) seems to be offered here:

http://www.springerlink.com/content/lxpnhaxnpfutfptt/

Unfortunately, I don't have access to the full text :(

"Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <glfqmp$267$1@fred.mathworks.com>...

>
> Actually what I gave is a least squares solution, Matt. If the two triples of points don't exactly match, it automatically gets a solution with the least squares error.
>

Hi Roger,

I have not read in detail your method, but
1. Do you estimate the translation vector?
2. Does the least-square 3x3 matrix is orthogonal?

It seems that what the article claim to be. It seems like optimal to me, wonder if anyone has implement it using Matlab.

Bruno

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <glfrf5$kdl$1@fred.mathworks.com>...
> "Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <glfqmp$267$1@fred.mathworks.com>...
>
> >
> > Actually what I gave is a least squares solution, Matt. If the two triples of points don't exactly match, it automatically gets a solution with the least squares error.
> >
>
> Hi Roger,
>
> I have not read in detail your method, but
> 1. Do you estimate the translation vector?
> 2. Does the least-square 3x3 matrix is orthogonal?
>
> It seems that what the article claim to be. It seems like optimal to me, wonder if anyone has implement it using Matlab.

Which article are you refering to, Bruno?

All methods discussed above ultimately give an orthogonal rotation matrix and a translation vector.

I have implemented Horn's method in MATLAB. Roger's method is the shortest to code, however, as it works directly in the space of matrices...

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <glfrf5$kdl$1@fred.mathworks.com>...
> Hi Roger,
>
> I have not read in detail your method, but
> 1. Do you estimate the translation vector?
> 2. Does the least-square 3x3 matrix is orthogonal?
>
> It seems that what the article claim to be. It seems like optimal to me, wonder if anyone has implement it using Matlab.
>
> Bruno

  Hello Bruno. As to question 1., it can easily be proved that the optimal translation is to move one or both centroids so they coincide, which is what is done in the method above. In that, both were moved to the origin. The result in the fourth column of T is the same however it is done.

  In 2. is your question whether the 3 x 3 matrix P is an orthogonal (unitary) matrix? The answer is yes, because it is product of two other unitary matrices, U and V', which are obtained with 'svd', (even if a change of sign of the 3rd column of V is done.) That is true, of course, providing there is no rescaling performed aferwards.

  The argument for this being a true least squares solution for vertices that don't match looks mathematically valid to me.

  In the Statistics Toolbox there is a function known as 'procrustes' which finds the least squares translation/reflection/rotation/scaling transformation, but I don't think it can be made to do only translation and rotation. That problem has arisen in the past in CSSM threads.

Roger Stafford

"Matt " <mjacobson.removethis@xorantech.com> wrote in message <glft7e$fjc$1@fred.mathworks.com>...
> "Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <glfrf5$kdl$1@fred.mathworks.com>...
> > "Roger Stafford" <ellieandrogerxyzzy@mindspring.com.invalid> wrote in message <glfqmp$267$1@fred.mathworks.com>...
> >
> > >
> > > Actually what I gave is a least squares solution, Matt. If the two triples of points don't exactly match, it automatically gets a solution with the least squares error.
> > >
> >
> > Hi Roger,
> >
> > I have not read in detail your method, but
> > 1. Do you estimate the translation vector?
> > 2. Does the least-square 3x3 matrix is orthogonal?
> >
> > It seems that what the article claim to be. It seems like optimal to me, wonder if anyone has implement it using Matlab.
>
> Which article are you refering to, Bruno?
>
> All methods discussed above ultimately give an orthogonal rotation matrix and a translation vector.
>
> I have implemented Horn's method in MATLAB. Roger's method is the shortest to code, however, as it works directly in the space of matrices...
>
>

I refer to Horn's method. I wasn't sure whereas Roger considered the translation in his method. Thanks for clear thing out.

Bruno

Thanks Roger.

Bruno

> It should be pointed out that the method I gave can be extended to more than three points and to sets X and Y that can't be matched exactly. The same procedure will give the nearest possible match in the least squares sense. It is a special case of the general "Procrustes" problem which also provides for rescaling.
>
> Roger Stafford
>

Hey Roger,

Thanks for the information with the links, and to Matt and Bruno as well for the information. I can see now why the third point is needed - and I believe my problem does in fact give me a third point - it was/is just not immediately apparent. I need to pour through the links and really formalize the problem now. (I think my third point comes from what I thought originally to be the trivial solution, where my known vector a3 is [0,0,0,1]. I do know the b3 vector associated with it, but I had originally assumed that this information may have been redundant - and thus wouldn't need it. We'll see though...)

Thanks very much for the good links as well - I'll pour over the details today and see if I can't get this problem fully worked out.

- Geoff