Discover MakerZone

MATLAB and Simulink resources for Arduino, LEGO, and Raspberry Pi

Learn more

Discover what MATLAB® can do for your career.

Opportunities for recent engineering grads.

Apply Today

Thread Subject:
euler angles?

Subject: euler angles?

From: Rafael Herrejon

Date: 4 Dec, 2008 18:02:02

Message: 1 of 20

Hello everybody

im having some trouble obtaining the euler angles of a "matrix", i wonder if you have any idea how to solve this. tried the symbolic math toolbox but it gets me an empty value. Any help would be very appreciated.

syms th1 th2 th3 %angles are symbolic
rotz=[cos(th3), -sin(th3),0; %define rotations in axes
    sin(th3),cos(th3),0;
    0 , 0, 1;]
roty=[cos(th2), 0, sin(th2);
             0, 1, 0;
    -sin(th2), 0, cos(th2)]

rotx=[1 0 0;
    0 cos(th1), -sin(th1);
    0 sin(th1), cos(th1);]

xyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z

vect=xyz*[0,0,1]'

vectA=vect(1)-C1
vectB=vect(2)-C2
vectC=vect(3)-C3

[th1, th2, th3]=solve(vectA,vectB,vectC)

where C1, C2 and C3 are know values, but it returns me

th1 =
[ empty sym ]
th2 =
     []
th3 =
     []

is there anyway to solve this? it doesnt have to be the symbolic solution, a numerical would work too.. just do not know how to do it or if is it even possible

Subject: euler angles?

From: Bruno Luong

Date: 4 Dec, 2008 19:44:03

Message: 2 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <gh95uq$4ng$1@fred.mathworks.com>...
> Hello everybody
>
> im having some trouble obtaining the euler angles of a "matrix", i wonder if you have any idea how to solve this. tried the symbolic math toolbox but it gets me an empty value. Any help would be very appreciated.
>
> syms th1 th2 th3 %angles are symbolic
> rotz=[cos(th3), -sin(th3),0; %define rotations in axes
> sin(th3),cos(th3),0;
> 0 , 0, 1;]
> roty=[cos(th2), 0, sin(th2);
> 0, 1, 0;
> -sin(th2), 0, cos(th2)]
>
> rotx=[1 0 0;
> 0 cos(th1), -sin(th1);
> 0 sin(th1), cos(th1);]
>
> xyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
>
> vect=xyz*[0,0,1]'
>
> vectA=vect(1)-C1
> vectB=vect(2)-C2
> vectC=vect(3)-C3
>
> [th1, th2, th3]=solve(vectA,vectB,vectC)
>
> where C1, C2 and C3 are know values, but it returns me
>
> th1 =
> [ empty sym ]
> th2 =
> []
> th3 =
> []
>
> is there anyway to solve this? it doesnt have to be the symbolic solution, a numerical would work too.. just do not know how to do it or if is it even possible

You want to get Euler angle. You know the rotation transforms (0,0,1)' to a known vector.

1. Take the cross product of those two vectors. It gives you the rotation axis and angle (be careful on the sign convention).

2. Use Rodiguez's rotation formula to build the matrix

3. Get the three basic euler from the matrix (There is a useful formula on Wikipedia)

There might have some short cut of the above method, but I let you the joy to discover all that.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 6 Dec, 2008 15:24:02

Message: 3 of 20

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <gh9bu2$egl$1@fred.mathworks.com>...
> "Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <gh95uq$4ng$1@fred.mathworks.com>...
> > Hello everybody
> >
> > im having some trouble obtaining the euler angles of a "matrix", i wonder if you have any idea how to solve this. tried the symbolic math toolbox but it gets me an empty value. Any help would be very appreciated.
> >
> > syms th1 th2 th3 %angles are symbolic
> > rotz=[cos(th3), -sin(th3),0; %define rotations in axes
> > sin(th3),cos(th3),0;
> > 0 , 0, 1;]
> > roty=[cos(th2), 0, sin(th2);
> > 0, 1, 0;
> > -sin(th2), 0, cos(th2)]
> >
> > rotx=[1 0 0;
> > 0 cos(th1), -sin(th1);
> > 0 sin(th1), cos(th1);]
> >
> > xyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
> >
> > vect=xyz*[0,0,1]'
> >
> > vectA=vect(1)-C1
> > vectB=vect(2)-C2
> > vectC=vect(3)-C3
> >
> > [th1, th2, th3]=solve(vectA,vectB,vectC)
> >
> > where C1, C2 and C3 are know values, but it returns me
> >
> > th1 =
> > [ empty sym ]
> > th2 =
> > []
> > th3 =
> > []
> >
> > is there anyway to solve this? it doesnt have to be the symbolic solution, a numerical would work too.. just do not know how to do it or if is it even possible
>
> You want to get Euler angle. You know the rotation transforms (0,0,1)' to a known vector.
>
> 1. Take the cross product of those two vectors. It gives you the rotation axis and angle (be careful on the sign convention).
>
> 2. Use Rodiguez's rotation formula to build the matrix
>
> 3. Get the three basic euler from the matrix (There is a useful formula on Wikipedia)
>
> There might have some short cut of the above method, but I let you the joy to discover all that.
>
> Bruno
Bruno

Thank you so much for your answer, I pretty much did what you said, but as you warned me, im having some troubles with the angles..

gworld=[0;0;1];
th1=-3.1350;
th2=-0.0280;
th3=3.1263;
rotz=[cos(th3), -sin(th3),0; %define rotations in axes
    sin(th3),cos(th3),0;
    0 , 0, 1;];
roty=[cos(th2), 0, sin(th2);
             0, 1, 0;
    -sin(th2), 0, cos(th2)];
rotx=[1 0 0;
    0 cos(th1), -sin(th1);
    0 sin(th1), cos(th1);];

rotxyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
vect=rotxyz*[0,0,1]';
vectorin1=(CROSS(vect,gworld));
rotob=(rodrigues(vectorin1));
%obtain euler angles
al2=-asin(rotob(3,1)); %%Rot_y
al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z

orig=[th1,th2,th3]
calc=[al1,al2,al3]

gets me

orig =
   -3.1350 -0.0280 3.1263
calc =
   -0.0062 0.0281 -0.0001

after some manipulation i found that adding some "-" in the equations

vectorin1=(CROSS(vect,gworld));
vectorin1(2)=-vectorin1(2);
rotob=-(rodrigues(vectorin1));
al2=-asin(rotob(3,1)); %%Rot_y
al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z
calc2=-[al1,al2,al3]

get me the correct angles.. main problem is, i dont really understand why do i have to add those minus signs (-).. does anybody can explain me or point me to a good reference?

Thank you so much in advance

Subject: euler angles?

From: Bruno Luong

Date: 6 Dec, 2008 19:18:02

Message: 4 of 20

Hi,

Can you post the rodrigues function?

Few premiminary remarks:
- Beware to compare two sets of euler angles, they might differ but still represent the same rotation
- Changing only ONE component of the cross product is fishy thing to do.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 13:03:02

Message: 5 of 20

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <ghej5a$294$1@fred.mathworks.com>...
> Hi,
>
> Can you post the rodrigues function?
>
> Few premiminary remarks:
> - Beware to compare two sets of euler angles, they might differ but still represent the same rotation
> - Changing only ONE component of the cross product is fishy thing to do.
>
> Bruno

Thank you for your response

the rodrigues function i am using is the one in camera calibration toolbox (http://www.vision.caltech.edu/bouguetj/calib_doc/), here it is.


function [out,dout]=rodrigues(in)

% RODRIGUES Transform rotation matrix into rotation vector and viceversa.
%
% Sintax: [OUT]=RODRIGUES(IN)
% If IN is a 3x3 rotation matrix then OUT is the
% corresponding 3x1 rotation vector
% if IN is a rotation 3-vector then OUT is the
% corresponding 3x3 rotation matrix
%

%%
%% Copyright (c) March 1993 -- Pietro Perona
%% California Institute of Technology
%%

%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!

%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet

%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet

% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007


[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;

if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
    theta = norm(in);
    if theta < eps
        R = eye(3);

        %if nargout > 1,

        dRdin = [0 0 0;
            0 0 1;
            0 -1 0;
            0 0 -1;
            0 0 0;
            1 0 0;
            0 1 0;
            -1 0 0;
            0 0 0];

        %end;

    else
        if n==length(in) in=in'; end; %% make it a column vec. if necess.

        %m3 = [in,theta]

        dm3din = [eye(3);in'/theta];

        omega = in/theta;

        %m2 = [omega;theta]

        dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];

        alpha = cos(theta);
        beta = sin(theta);
        gamma = 1-cos(theta);
        omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
        A = omega*omega';

        %m1 = [alpha;beta;gamma;omegav;A];

        dm1dm2 = zeros(21,4);
        dm1dm2(1,4) = -sin(theta);
        dm1dm2(2,4) = cos(theta);
        dm1dm2(3,4) = sin(theta);
        dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0]';

        w1 = omega(1);
        w2 = omega(2);
        w3 = omega(3);

        dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
        dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
        dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];

        R = eye(3)*alpha + omegav*beta + A*gamma;

        dRdm1 = zeros(9,21);

        dRdm1([1 5 9],1) = ones(3,1);
        dRdm1(:,2) = omegav(:);
        dRdm1(:,4:12) = beta*eye(9);
        dRdm1(:,3) = A(:);
        dRdm1(:,13:21) = gamma*eye(9);

        dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;


    end;
    out = R;
    dout = dRdin;

    %% it is prob. a rot matr.
elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
        & (abs(det(in)-1) < bigeps))
    R = in;

    % project the rotation matrix to SO(3);
    [U,S,V] = svd(R);
    R = U*V';

    tr = (trace(R)-1)/2;
    dtrdR = [1 0 0 0 1 0 0 0 1]/2;
    theta = real(acos(tr));


    if sin(theta) >= 1e-4,

        dthetadtr = -1/sqrt(1-tr^2);

        dthetadR = dthetadtr * dtrdR;
        % var1 = [vth;theta];
        vth = 1/(2*sin(theta));
        dvthdtheta = -vth*cos(theta)/sin(theta);
        dvar1dtheta = [dvthdtheta;1];

        dvar1dR = dvar1dtheta * dthetadR;


        om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';

        dom1dR = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0];

        % var = [om1;vth;theta];
        dvardR = [dom1dR;dvar1dR];

        % var2 = [om;theta];
        om = vth*om1;
        domdvar = [vth*eye(3) om1 zeros(3,1)];
        dthetadvar = [0 0 0 0 1];
        dvar2dvar = [domdvar;dthetadvar];


        out = om*theta;
        domegadvar2 = [theta*eye(3) om];

        dout = domegadvar2 * dvar2dvar * dvardR;


    else
        if tr > 0; % case norm(om)=0;

            out = [0 0 0]';

            dout = [0 0 0 0 0 1/2 0 -1/2 0;
                0 0 -1/2 0 0 0 1/2 0 0;
                0 1/2 0 -1/2 0 0 0 0 0];
        else

            % case norm(om)=pi;
            if(0)

                %% fixed April 6th by Bouguet -- not working in all cases!
                out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
                %keyboard;

            else

                % Solution by Mike Burl on Feb 27, 2007
                % This is a better way to determine the signs of the
                % entries of the rotation vector using a hash table on all
                % the combinations of signs of a pairs of products (in the
                % rotation matrix)

                % Define hashvec and Smat
                hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11];
                Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1;
                    1,-1,-1; 1,-1,1];

                M = (R+eye(3,3))/2;
                uabs = sqrt(M(1,1));
                vabs = sqrt(M(2,2));
                wabs = sqrt(M(3,3));

                mvec = [M(1,2), M(2,3), M(1,3)];
                syn = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function
                hash = syn * [9; 3; 1];
                idx = find(hash == hashvec);
                svec = Smat(idx,:)';

                out = theta * [uabs; vabs; wabs] .* svec;

            end;

            if nargout > 1,
                fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
                dout = NaN*ones(3,9);
            end;
        end;
    end;

else
    error('Neither a rotation matrix nor a rotation vector were provided');
end;

return;

%% test of the Jacobians:

%%%% TEST OF dRdom:
om = randn(3,1);
dom = randn(3,1)/1000000;

[R1,dR1] = rodrigues(om);
R2 = rodrigues(om+dom);

R2a = R1 + reshape(dR1 * dom,3,3);

gain = norm(R2 - R1)/norm(R2 - R2a)

%%% TEST OF dOmdR:
om = randn(3,1);
R = rodrigues(om);
dom = randn(3,1)/10000;
dR = rodrigues(om+dom) - R;

[omc,domdR] = rodrigues(R);
[om2] = rodrigues(R+dR);

om_app = omc + domdR*dR(:);

gain = norm(om2 - omc)/norm(om2 - om_app)


%%% OTHER BUG: (FIXED NOW!!!)

omu = randn(3,1);
omu = omu/norm(omu)
om = pi*omu;
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

%%% NORMAL OPERATION

om = randn(3,1);
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

return

% Test: norm(om) = pi

u = randn(3,1);
u = u / sqrt(sum(u.^2));
om = pi*u;
R = rodrigues(om);

R2 = rodrigues(rodrigues(R));

norm(R - R2)

and you are right, changing just one column is very fishy, so i am wondering why only doing this works..

Subject: euler angles?

From: Bruno Luong

Date: 7 Dec, 2008 13:29:02

Message: 6 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghghi6$oqk$1@fred.mathworks.com>...

>
> and you are right, changing just one column is very fishy, so i am wondering why only doing this works..

You think it works but I'm sure it doesn't.

Before I take a look of the code (I'm busy with something else right now), you can make few basic chekcs please:
1. apply the Rodrigues's rotation matrix to your vector (0,0,1)', and see if you get the wanted vector.
2. Rebuild the three euler matrices from the euler angles you get, multiply them and check if it gives the Rodrigues's matrix.

If both steps are OK, then your result is correct.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 14:29:02

Message: 7 of 20

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <ghgj2u$cs1$1@fred.mathworks.com>...
> "Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghghi6$oqk$1@fred.mathworks.com>...
>
> >
> > and you are right, changing just one column is very fishy, so i am wondering why only doing this works..
>
> You think it works but I'm sure it doesn't.
>
> Before I take a look of the code (I'm busy with something else right now), you can make few basic chekcs please:
> 1. apply the Rodrigues's rotation matrix to your vector (0,0,1)', and see if you get the wanted vector.
> 2. Rebuild the three euler matrices from the euler angles you get, multiply them and check if it gives the Rodrigues's matrix.
>
> If both steps are OK, then your result is correct.
>
> Bruno

Thank you for your fast response, ok im not sure if it works or not, doing the tests you said

vectorin1=(CROSS(vect,gworld));
%vectorin1(2)=-vectorin1(2);
rotob=-(rodrigues(vectorin1));

you are right, if i add the line "vectorin1(2)=-vectorin1(2);"

testing=rotob*[0,0,1]'

gets me

    0.0281 -0.0062 -0.9996
but they should be
   -0.0281 -0.0062 -0.9996

do you think the rodrigues function is wrong implemented?

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 14:40:17

Message: 8 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghgmje$j6d$1@fred.mathworks.com>...
> "Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <ghgj2u$cs1$1@fred.mathworks.com>...
> > "Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghghi6$oqk$1@fred.mathworks.com>...
> >
> > >
> > > and you are right, changing just one column is very fishy, so i am wondering why only doing this works..
> >
> > You think it works but I'm sure it doesn't.
> >
> > Before I take a look of the code (I'm busy with something else right now), you can make few basic chekcs please:
> > 1. apply the Rodrigues's rotation matrix to your vector (0,0,1)', and see if you get the wanted vector.
> > 2. Rebuild the three euler matrices from the euler angles you get, multiply them and check if it gives the Rodrigues's matrix.
> >
> > If both steps are OK, then your result is correct.
> >
> > Bruno
>
> Thank you for your fast response, ok im not sure if it works or not, doing the tests you said
>
> vectorin1=(CROSS(vect,gworld));
> %vectorin1(2)=-vectorin1(2);
> rotob=-(rodrigues(vectorin1));
>
> you are right, if i add the line "vectorin1(2)=-vectorin1(2);"
>
> testing=rotob*[0,0,1]'
>
> gets me
>
> 0.0281 -0.0062 -0.9996
> but they should be
> -0.0281 -0.0062 -0.9996
>
> do you think the rodrigues function is wrong implemented?

sorry i meant that if i add that line the result is wrong, but anyways if i dont add that line the angles that i get are positive
0.0281 0.0062 0.9996

so im adding a '-' after calculating the rodrigues... is this wrong? do you think the rodrigues function is implemented wrongly?

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 15:07:01

Message: 9 of 20

sorry for writting several response for every step i did instead of only one with everything included, ill try to write the later in this one.

from the two steps
> 1. apply the Rodrigues's rotation matrix to your vector (0,0,1)', and see if you get the wanted vector.

vectorin1=(CROSS(vect,gworld));
rotob=rodrigues(vectorin1);
testing=rotob*[0,0,1]'

NO!, i dont get the wanted vector, it should be [-0.0281 -0.0062 -0.9996] but i get [0.0281 0.0062 0.9996] which is a '-' away

> 2. Rebuild the three euler matrices from the euler angles you get, multiply them and check if it gives the Rodrigues's matrix.

vectorin1=(CROSS(vect,gworld));
rotob=rodrigues(vectorin1);
testing=rotob*[0,0,1]'
al2=-asin(rotob(3,1)); %%Rot_y
al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z

rotz2=[cos(al3), -sin(al3),0; %define rotations in axes
    sin(al3),cos(al3),0;
    0 , 0, 1;];
roty2=[cos(al2), 0, sin(al2);
             0, 1, 0;
    -sin(al2), 0, cos(al2)];
rotx2=[1 0 0;
    0 cos(al1), -sin(al1);
    0 sin(al1), cos(al1);];

rotxyz2=[rotz2*roty2*rotx2]
rotob

YES!! euler matrices multiplied gets me the Rodrigues' matrix.
rotxyz2 =
    0.9996 -0.0001 0.0281
   -0.0001 1.0000 0.0062
   -0.0281 -0.0062 0.9996
rotob =
    0.9996 -0.0001 0.0281
   -0.0001 1.0000 0.0062
   -0.0281 -0.0062 0.9996

BUT, rotxyz2 from obtained angles(=rotob) is different to rotxyz from original angles :(

th1=-3.1350;
th2=-0.0280;
th3=3.1263;
rotz=[cos(th3), -sin(th3),0; %define rotations in axes
    sin(th3),cos(th3),0;
    0 , 0, 1;];
roty=[cos(th2), 0, sin(th2);
             0, 1, 0;
    -sin(th2), 0, cos(th2)];
rotx=[1 0 0;
    0 cos(th1), -sin(th1);
    0 sin(th1), cos(th1);];

rotxyz=[rotz*roty*rotx]

rotxyz =
   -0.9995 0.0151 -0.0281
    0.0153 0.9999 -0.0062
    0.0280 -0.0066 -0.9996

so i really have no clue whats wrong :(

Subject: euler angles?

From: Bruno Luong

Date: 7 Dec, 2008 15:11:02

Message: 10 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghgn8h$qni$1@fred.mathworks.com>...
> "Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message
>
> so im adding a '-' after calculating the rodrigues... is this wrong? do you think the rodrigues function is implemented wrongly?

No, I doubt it. It just means you need to swap the two input vectors of the cross product.

Remember, the best way for not making mistake in the determine the sign in your program is - uno think very hard what the sign should be, then - secondo reverse what you think is correct.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 15:34:02

Message: 11 of 20

> No, I doubt it. It just means you need to swap the two input vectors of the cross product.
>
> Remember, the best way for not making mistake in the determine the sign in your program is - uno think very hard what the sign should be, then - secondo reverse what you think is correct.

Thank you, but if i swap the two input vectors, the values are still wrong... they should be

   -0.0281 -0.0062 -0.9996

but i get

   -0.0281 -0.0062 0.9996

so, im still clueless... :(

Subject: euler angles?

From: Bruno Luong

Date: 7 Dec, 2008 15:39:02

Message: 12 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghgqda$1it$1@fred.mathworks.com>...
> > No, I doubt it. It just means you need to swap the two input vectors of the cross product.
> >
> > Remember, the best way for not making mistake in the determine the sign in your program is - uno think very hard what the sign should be, then - secondo reverse what you think is correct.
>
> Thank you, but if i swap the two input vectors, the values are still wrong... they should be
>
> -0.0281 -0.0062 -0.9996
>
> but i get
>
> -0.0281 -0.0062 0.9996
>
> so, im still clueless... :(

Either your CROSS function or RODRIGUES function is wrong.

CROSS should gives negative when you swap the inputs, and RODRIGUES should gives negative matrix.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 7 Dec, 2008 20:05:03

Message: 13 of 20

> Either your CROSS function or RODRIGUES function is wrong.
>
> CROSS should gives negative when you swap the inputs, and RODRIGUES should gives negative matrix.
>
> Bruno

mmm unfortunately, there are both correct as far as i could check

CROSS function is the one build in Matlab, and seems to be correct

>vectorin1=(CROSS(gworld,vect))'
vectorin1 = [ 0.0062; -0.0281; 0]

>rotob=rodrigues(vectorin1) %%% checked rodrigues with OpenCV routine cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ), it gives the same result!!

rotob =
    0.9996 -0.0001 -0.0281
   -0.0001 1.0000 -0.0062
    0.0281 0.0062 0.9996

>vect'
ans =
  -0.0281 -0.0062 -0.9996

>ch=[rotob*gworld]'
ch =
   -0.0281 -0.0062 0.9996 %%% angles are wrong sign


>al2=-asin(rotob(3,1)); %%Rot_y
>al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
>al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z
>rotz2=[cos(al3), -sin(al3),0; %define rotations in axes
    sin(al3),cos(al3),0;
    0 , 0, 1;];
>roty2=[cos(al2), 0, sin(al2);
             0, 1, 0;
    -sin(al2), 0, cos(al2)];
>rotx2=[1 0 0;
    0 cos(al1), -sin(al1);
    0 sin(al1), cos(al1);];
>rotxyz2=[rotz2*roty2*rotx2] %

rotxyz2 =
    0.9996 -0.0001 -0.0281
   -0.0001 1.0000 -0.0062
    0.0281 0.0062 0.9996

>vectorin1=(CROSS(vect,gworld))'

vectorin1 = [ -0.0062; 0.0281; 0]

>rotob=rodrigues(vectorin1) %%% checked rodrigues with OpenCV routine cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 ), it gives the same result!!

rotob =
    0.9996 -0.0001 0.0281
   -0.0001 1.0000 0.0062
   -0.0281 -0.0062 0.9996

ch =
    0.0281 0.0062 0.9996

ans =
   -0.0281 -0.0062 -0.9996 %%angles are all negative

rotxyz2 =
    0.9996 -0.0001 0.0281
   -0.0001 1.0000 0.0062
   -0.0281 -0.0062 0.9996

what do you think might be wrong?

Subject: euler angles?

From: Bruno Luong

Date: 7 Dec, 2008 21:15:06

Message: 14 of 20

gworld=[0;0;1];
th1=-3.1350;
th2=-0.0280;
th3=3.1263;
Here is the first part of getting the rotation matrix:
I let you figure out the euler angles:

rotz=[cos(th3), -sin(th3),0; %define rotations in axes
    sin(th3),cos(th3),0;
    0 , 0, 1;];
roty=[cos(th2), 0, sin(th2);
             0, 1, 0;
    -sin(th2), 0, cos(th2)];
rotx=[1 0 0;
    0 cos(th1), -sin(th1);
    0 sin(th1), cos(th1);];

rotxyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
vect=rotxyz*gworld
vectorin1=cross(gworld,vect)/(norm(gworld)*norm(vect));
nv1=min(max(norm(vectorin1),-1),1);
theta=asin(nv1)
if dot(gworld,vect)<0
    theta=pi-theta;
end
rotob=rodrigues(vectorin1/nv1*theta)

rotob*gworld % should be close to vect

% Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 8 Dec, 2008 16:18:02

Message: 15 of 20

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <ghhecq$lr5$1@fred.mathworks.com>...
> gworld=[0;0;1];
> th1=-3.1350;
> th2=-0.0280;
> th3=3.1263;
> Here is the first part of getting the rotation matrix:
> I let you figure out the euler angles:
>
> rotz=[cos(th3), -sin(th3),0; %define rotations in axes
> sin(th3),cos(th3),0;
> 0 , 0, 1;];
> roty=[cos(th2), 0, sin(th2);
> 0, 1, 0;
> -sin(th2), 0, cos(th2)];
> rotx=[1 0 0;
> 0 cos(th1), -sin(th1);
> 0 sin(th1), cos(th1);];
>
> rotxyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
> vect=rotxyz*gworld
> vectorin1=cross(gworld,vect)/(norm(gworld)*norm(vect));
> nv1=min(max(norm(vectorin1),-1),1);
> theta=asin(nv1)
> if dot(gworld,vect)<0
> theta=pi-theta;
> end
> rotob=rodrigues(vectorin1/nv1*theta)
>
> rotob*gworld % should be close to vect
>
Thank you for the helping me getting the rotation matrix, im trying to follow but cant understand what are you doing when dividing and multiplying by norm.
As far as i know, to obtain the euler angles of a rotation matrix RxRyRz the following applies

al2=-asin(rotob(3,1)); %%Rot_y
al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z

but after this multiplications by norm.. i dont understand the relationship but..

the euler angles of this matrix rotob
al1=3.1354; al2=-0.0281; al3=-2.7095

are different from the original angles
th1=-3.1350; th2=-0.0280; th3=3.1263;

Subject: euler angles?

From: Bruno Luong

Date: 8 Dec, 2008 18:09:02

Message: 16 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghjhbq$nmk$1@fred.mathworks.com>...

>
> al2=-asin(rotob(3,1)); %%Rot_y
> al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
> al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z
>

The above doesn't seeem to be 100% correct when I multiply basic rotation matrices using al1, al2 and al3, I can't get rotob.

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 8 Dec, 2008 19:54:02

Message: 17 of 20

sorry, i think the euler angle calculation from a given matrix is correct

gworld=[0;0;1];

th1=-3.1350;
th2=-0.0280;
th3=3.1263;
th=[th1,th2,th3];
rotz=[cos(th3), -sin(th3),0; %define rotations in axes
    sin(th3),cos(th3),0;
    0 , 0, 1;];
roty=[cos(th2), 0, sin(th2);
             0, 1, 0;
    -sin(th2), 0, cos(th2)];
rotx=[1 0 0;
    0 cos(th1), -sin(th1);
    0 sin(th1), cos(th1);];

rotxyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z
vect=rotxyz*gworld

vectorin1=cross(gworld,vect)/(norm(gworld)*norm(vect));
nv1=min(max(norm(vectorin1),-1),1);
theta=asin(nv1);
if dot(gworld,vect)<0
    theta=pi-theta;
end
rotob=rodrigues(vectorin1/nv1*theta)
rotob*gworld % should be close to vect


al2=-asin(rotob(3,1)); %%Rot_y
al1=atan2(rotob(3,2)/cos(al2),rotob(3,3)/cos(al2)); %%Rot_x
al3=atan2(rotob(2,1)/cos(al2),rotob(1,1)/cos(al2)); %%Rot_z
rotz2=[cos(al3), -sin(al3),0; %define rotations in axes
    sin(al3),cos(al3),0;
    0 , 0, 1;];
roty2=[cos(al2), 0, sin(al2);
             0, 1, 0;
    -sin(al2), 0, cos(al2)];
rotx2=[1 0 0;
    0 cos(al1), -sin(al1);
    0 sin(al1), cos(al1);];
rotxyz2=[rotz2*roty2*rotx2] %

al1, al2 and al3 are the correct euler angles from rotobo

rotxyz2=[rotz2*roty2*rotx2] %
is equal to
rotob

but rotxyz2 and rotob are different to the original rotxyz matrix

rotxyz=[rotz*roty*rotx] %rotation matrix first in x, then y and finally z

what could be wrong? btw, thank you for the support

Subject: euler angles?

From: Bruno Luong

Date: 9 Dec, 2008 11:17:02

Message: 18 of 20

"Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghju0q$1j5$1@fred.mathworks.com>...

>
> but rotxyz2 and rotob are different to the original rotxyz matrix
>

That because there is no unique rotation that maps "gworld" to "vect". There is infact an infinity solution, each correspond to where the rotation axis that can be a combination of the cross product vector (perpendicular to gworld and vec) and the mean of gworld" to "vect".

Bruno

Subject: euler angles?

From: Rafael Herrejon

Date: 10 Dec, 2008 07:05:13

Message: 19 of 20

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <ghlk3e$ip8$1@fred.mathworks.com>...
> "Rafael Herrejon" <rafael.erasethis@ic.is.tohoku.ac.jp> wrote in message <ghju0q$1j5$1@fred.mathworks.com>...
>
> >
> > but rotxyz2 and rotob are different to the original rotxyz matrix
> >
>
> That because there is no unique rotation that maps "gworld" to "vect". There is infact an infinity solution, each correspond to where the rotation axis that can be a combination of the cross product vector (perpendicular to gworld and vec) and the mean of gworld" to "vect".
>
> Bruno

thank you for the answer
what if "gworld" is the gravity vector [0;0;g] and "vect" are the components of the gravity for that reference frame?

Subject: euler angles?

From: Bruno Luong

Date: 10 Dec, 2008 07:40:24

Message: 20 of 20

>
> thank you for the answer
> what if "gworld" is the gravity vector [0;0;g] and "vect" are the components of the gravity for that reference frame?

This is no longer a math question but the question concerning the formulation and modelisation of your application, and certainly do not know it to be able to answer.

Just a quick general remark: depending on the selection of the rotation axis among infinity possible solutions, when mapped the vertical vector (gworld), you might get different yaw projection on the reference frame of two horizontal axis (e.g., latitude, longitude). You should take a look whereas if there is another constraint or condition (on yaw, north/south) you might forget to use for getting a unique rotation.

Bruno

Tags for this Thread

What are tags?

A tag is like a keyword or category label associated with each thread. Tags make it easier for you to find threads of interest.

Anyone can tag a thread. Tags are public and visible to everyone.

Contact us