Find rotation matrix using fsolve

Hello,

I'm currently writing my thesis and I'm stuck. The problem is as follows: given the coordinates of three points in two diffrent coordinate systems find rotation matrix (defined by three angles) and translation vector.

Coordinates of three points are enough to write nine equations with six independent variables.

Where: p - translation vector; R - rotation matrix; 't'/'f' upper indice - first/second coordinate system.

I'm using fsolve to find solution to this problem and it converges to a solution everytime but it's not always the "right" solution. Below you'll see the example of what I'm writing about. I have a kinematic model of a 1-dof device that allows me to calculate the set of the coordinates of three points mentioned earlier given the value of one independent variable (different angle - not mentioned earlier). Using that I should be able to plot for example gamma(alpha) funtion and given the fact that I used kinematic model to calculate the coordinates it should be continuous. Here are the results:

This one is already after: gamma(i)=atan2(sin(gamma(i)),cos(gamma(i)))

I've been using atan2 to filter solutions like x0+360deg, etc. But I can't deal with those noncontinuous parts which are clearly not the right solution. Sometimes I get lucky and this funtion is continuous but I can't rely on luck cause I'm using this result in parameter estimation (using ga). I need this function to be continuous because next step is using polyfit on it.

The implementation of this problem in Matlab is as follows:

function [ F ] = eqsysRot3( x )
% x1=beta
% x2=gamma
% x3=x
% x4=y
% x5=z
% x6=alpha

global b1 b2 b3
global b1f b2f b3f

R=[cos(x(2))*sin(x(1))   -cos(x(6))*sin(x(2))-cos(x(2))*sin(x(6))*cos(x(1))   sin(x(6))*sin(x(2))-cos(x(2))*cos(x(6))*cos(x(1));
 sin(x(2))*sin(x(1))   cos(x(6))*cos(x(2))-sin(x(2))*sin(x(6))*cos(x(1))    -cos(x(2))*sin(x(6))-cos(x(6))*cos(x(1))*sin(x(2));
 cos(x(1))             sin(x(1))*sin(x(6))                                   cos(x(6))*sin(x(1))];                             
p=[x(3); x(4); x(5)];

b1s=R*b1f+p;
b2s=R*b2f+p;
b3s=R*b3f+p;
b4s=R*b3f+p;

F(1)=b1s(1)-b1(1);
F(2)=b1s(2)-b1(2);
F(3)=b1s(3)-b1(3);
F(4)=b2s(1)-b2(1);
F(5)=b2s(2)-b2(2);
F(6)=b2s(3)-b2(3);
F(7)=b3s(1)-b3(1);
F(8)=b3s(2)-b3(2);
F(9)=b3s(3)-b3(3);
end

And then in the other file fsolve is called:

start2=[0 0 0 0 0 0];
options = optimset('Display','iter','MaxFunEvals',900,'MaxIter',700);
x2=fsolve(@eqsysRot3, start2, options);

I've been experimenting with using GA to find a good starting vector for fsolve but it didn't do much.

Thanks for your time and regards,

Adam

Answer 1

[Matt J]

There is a closed-form solution for this, implemented here

http://www.mathworks.com/matlabcentral/fileexchange/26186-absolute-orientation-horns-method

This will give you the rotation matrix directly. You can then decompose it into angles, if you really need them, using any of the various conventions described here

http://en.wikipedia.org/wiki/Euler_angles

Answer 2

[Iain]

When some angles get close to values near +/-90 degrees, using rotation matrices to calculate euler angles is prone to error

I would suggest that you look at using quaternions instead, they are more complicated to understand, but they do avoid much of the numerical funnies that euler angles give.

[Matt J]

The routine at the link I gave will also return the quaternion representation of the rotation.