Symbolic에 의한 Jacobian 연산

물체 추적이나 필터 설계에서 Jacobian을 계산해야 할 경우가 많다.  자세 행렬의 Jacobian을 구할 경우 회전 행렬을 각도 변수에 대해 편미분해야 하며 손으로 계산하기에는 복잡하다.  Matlab의 symbolic연산을 이용하여 Qurternion을 이용하는 회전 행렬의 편미분을 계산해 보자.

% Symbolic 연산 테스트

% Qurternion의 Jacobian 계산

% 01/2014, funMV

clear; clc;

syms alpa; % 변수 alpa를 symbol로 선언 

syms beta;

syms gama;

cos_alpa = cos(alpa/2); sin_alpa=sin(alpa/2);

cos_beta = cos(beta/2); sin_beta=sin(beta/2);

cos_gama = cos(gama/2); sin_gama=sin(gama/2);

q1=cos_alpa*cos_beta*sin_gama-sin_alpa*sin_beta*cos_gama;

q2=cos_alpa*sin_beta*cos_gama+sin_alpa*cos_beta*sin_gama;

q3=sin_alpa*cos_beta*cos_gama-cos_alpa*sin_beta*sin_gama;

q4=cos_alpa*cos_beta*cos_gama+sin_alpa*sin_beta*sin_gama;

%{

q1 =

cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)

%} 

jacobian(q1,alpa)

% -(cos(alpa/2)*cos(gama/2)*sin(beta/2))/2-(cos(beta/2)*sin(alpa/2)*sin(gama/2))/2 

jacobian(q1,beta)

% -(cos(beta/2)*cos(gama/2)*sin(alpa/2))/2-(cos(alpa/2)*sin(beta/2)*sin(gama/2))/2 

jacobian(q1,gama)

% (sin(alpa/2)*sin(beta/2)*sin(gama/2))/2+(cos(alpa/2)*cos(beta/2)*cos(gama/2))/2

jacobian(q2,alpa)

% (cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2

jacobian(q2,beta)

% (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2 - (sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 

jacobian(q2,gama)

% (cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2 

% product of two quaternion b and q

syms b1;

syms b2;

syms b3;

syms b4;

qq1 = b1*q1-[b2 b3 b4]*[q2; q3; q4];

qq2 = (b1*[q2 q3 q4]+q1*[b2 b3 b4])+(cross([b2 b3 b4]',[q2 q3 q4]'))';

%{

qq1 =

  b1*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

- b2*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

- b3*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

- b4*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

  

qq2(1)= b1*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

   + b2*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

   + b3*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2)) 

   - b4*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)), 

qq2(2)= b1*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

   - b2*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

   + b3*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

   + b4*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)), 

qq2(3)= b1*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

   + b2*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

   - b3*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

   + b4*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2))

 %}

jacobian(qq1, b1)

%cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)

jacobian(qq1, b2)

%- cos(alpa/2)*cos(gama/2)*sin(beta/2) - cos(beta/2)*sin(alpa/2)*sin(gama/2)   

jacobian(qq1, b3)

%cos(alpa/2)*sin(beta/2)*sin(gama/2) - cos(beta/2)*cos(gama/2)*sin(alpa/2)

jacobian(qq1, b4)

%- sin(alpa/2)*sin(beta/2)*sin(gama/2) -%cos(alpa/2)*cos(beta/2)*cos(gama/2)

jacobian(qq1, alpa)

%{

b4*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b2*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b3*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

- b1*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2)

%}

jacobian(qq2, b1)

% cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)

% cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)

% sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2)

jacobian(qq2, alpa)

%{

 b1*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b2*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2) 

- b3*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b4*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2),

 b1*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

+ b2*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b3*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2) 

+ b4*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2),

 b2*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

- b1*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b3*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b4*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2)

%} 

Quaternion matrix

% Simulation for rotation matrix
% Coded by funMV. 12/2013
clear; clc;

% x축에 대해 45도 회전
% 회전 행렬은 표준 축이 회전된 방향 벡터(unit vector)이다.

alpa = 45; % x축 기준
beta = 30; % y축
gama = 60; % z축

p2a = pi/180;

% (1) 직접 3x3크기의 R행렬을 만듬
rr1 = rot(alpa*p2a, beta*p2a, gama*p2a);

% (2) rx, ry, rz로 만드나 결과는 동일
ssx = sin(alpa*p2a);
csx = cos(alpa*p2a);
ssy = sin(beta*p2a);
csy = cos(beta*p2a);
ssz = sin(gama*p2a);
csz = cos(gama*p2a);

rx = [1 0 0; 0 csx -ssx; 0 ssx csx];
%{
rx =

    1.0000         0         0
         0    0.7071   -0.7071
         0    0.7071    0.7071
%}
% x축을 중심으로 회전 시키므로 x축 방향은 불변
% 회전 각의 방향은 오른손 나사 방향으로 45도 회전
% 기존 y축은 [0 1 0]에서 [0  0.7071 0.7071]로 변경
% 기존 z축은 [0 0 1]에서 [0 -0.7071 0.7071]로 변경
% 회전 행렬 rx는 회전 후의 세 축의 방향 벡터(unit three vectors)

ry = [csy 0 ssy; 0 1 0; -ssy 0 csy];
rz = [csz -ssz 0; ssz csz 0; 0 0 1];
rr2 = rz*ry*rx;

% R(3x3)행렬에서 다시 3개의 각도 벡터를 추출
v = rpy(rr2);
v/p2a  % 45, 40, 60이 나옴


% 3개의 각도 벡터에서 quaternion을 계산 
q = angle2quat(v(1),v(2),v(3)); % angle to quaternion
% norm(q) = 1

% quaternion에서 다시 3개의 각도를 추출
rpy(q2r(q))/p2a % 세개의 각도 값 출력 순서: 60,30,45도로 나옴)

% matlab내부 함수를 이용해 각도 값 추출
[r1,r2,r3] = quat2angle(q); %  45,30,60도 나옴
r1/p2a
r2/p2a
r3/p2a



% 두 연속 회전에 의한 각도 값 비교
% quaternion의 연속 적용과 R회전 행렬의 연속 적용 후
% 결과 값이 같은지를 비교한다. 
% 새로운 3개의 각도 벡터 정의
dom = [2, 5, 7]*p2a;
rr3 = rot(dom(1),dom(2),dom(3)); % 회전 행렬 생성

q2 = angle2quat(dom(1), dom(2), dom(3)); % quaternion 계산
qp1 = quatmultiply(q, q2); % 두 quaternion의 곱
%qp2 = qprod(q,q2); % 두 quaternion의 곱
[r4,r5,r6] =quat2angle(qp1); % quaternion에서 각도 추출
r4/p2a
r5/p2a
r6/p2a     % 51.1953, 30.6278, 70.0420   

rr4 = rr3*rr2; % 하나의 강체에 두 회전의 연속 적용
v3 = rpy(rr4); % 즉, R행렬의 연속 적용이나 quaternion의 곱에 의한
v3/p2a         % 회전의 연속 적용이나 결과는 동일함 









function v_rot=rpy(m_rot)
% function v_rot=rpy(m_rot)
%
% Description
%  Computes the orientation vector (in rads) corresponding
%  to a rotation matrix. (dim 3)
%  [psi, theta, phi]
%  R -> (alpa, beta, gama)로 변환 함수 
%  v_rot1 = alpa(x), v_rot2=beta(y), v_rot3=gama(z)
%  에 대응되는 각도임 
% The input datum is:
% - m_rot.- a rotation matrix
%
% The return value is:
%  the rpy vector
v_rot(3)=angle(m_rot(2,1)*i+m_rot(1,1));
v_rot(2)=angle(-m_rot(3,1)*i+m_rot(1,1)*cos(v_rot(3))+m_rot(2,1)*sin(v_rot(3)));
v_rot(1)=angle(m_rot(3,2)*i+m_rot(3,3));
return;



function r=rot(psi,theta,phi);
% function r=rot(psi,theta,phi)
%
% Description
%  Computes the rotarion matrix corresponding to a oritentation vector
% The input data are:
% - psi   .- the psi angle (radians)
% - theta .- the theta angle (radians)
% - phi   .- the phi angle (radians)
%
% The return value is
% the rotation matrix (dim 3x3)
% Coded by 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Programmer   : Jose Maria Martinez                                       
%% Languaje     : Matlab 4.2 c                                              
%% Date         : February 1996                                             
%% Status       : Prueba                                                    
%% History : 16-8-95 creation                                       
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cosphi=cos(phi);
sinphi=sin(phi);
costheta=cos(theta);
sintheta=sin(theta);
cospsi=cos(psi);
sinpsi=sin(psi);

r=zeros(3,3);
r(1,1)=cosphi*costheta;
r(1,2)=cosphi*sintheta*sinpsi-sinphi*cospsi;
r(1,3)=cosphi*sintheta*cospsi+sinphi*sinpsi;
r(2,1)=sinphi*costheta;
r(2,2)=sinphi*sintheta*sinpsi+cosphi*cospsi;
r(2,3)=sinphi*sintheta*cospsi-cosphi*sinpsi;
r(3,1)=-sintheta;
r(3,2)=costheta*sinpsi;
r(3,3)=costheta*cospsi;
return;



%-----------------------------------------------------------------------
% Authors:  Javier Civera -- jcivera@unizar.es 
%           J. M. M. Montiel -- josemari@unizar.es
% Robotics, Perception and Real Time Group
%           Institute of Engineering Research (I3A)
% Universidad de Zaragoza, 50018, Zaragoza, Spain
% Date   :  May 2010
%-----------------------------------------------------------------------
function R=q2r(q)

x=q(2);
y=q(3);
z=q(4);
r=q(1);

R=[r*r+x*x-y*y-z*z  2*(x*y -r*z)     2*(z*x+r*y)
   2*(x*y+r*z)      r*r-x*x+y*y-z*z  2*(y*z-r*x)
   2*(z*x-r*y)      2*(y*z+r*x)      r*r-x*x-y*y+z*z];



% Product of two quaternion
function qp=qprod(q,p)
a=q(1);
v=reshape(q(2:4),3,1);
x=p(1);
u=reshape(p(2:4),3,1);

qp = [a*x-v'*u, (a*u+x*v)' + cross(v,u)'];





References
[1] 오일러각과 회전행렬(Euler Angles and Rotation Matrix)
[2] 각속도(Angular Velocity)와 오일러각, 회전행렬 간의 관계
[3] 쿼터니언(Quaternion)과 오일러각, 회전행렬 간의 관계
[4] 회전 보간: 쿼터니언(Quaternion)을 사용한 Slerp(구면 선형 보간)

두 축 사이의 변환 관계

공간에 두 개의 좌표 축이 있다:

1. 두 축에서 바라본 한 점 X의 좌표 사이의 관계는 다음과 같다.

이 식은 두 축 사이의 회전은 없다고 가정한 경우이다.
Oc에서 X로 가는 벡터는 Oc에서 Ow로 가서 다시 Ow에서 X로 가는 벡터의 합이다.
Ow 기준에서 보면 Oc의 좌표는 tw이다. 따라서 -tw 이동하면 두 축은 일치 된다.
(tc가 아니라 tw로 주어 졌다.)

좌표이동을 선형변환으로 표현하였다. tilt는 3-vector이다.


2. 회전을 표현하는 방법은 2가지 이다.  두 좌표의 자세가 다르다고 가정하자.
먼저 C축을 돌리는 경우이다.  즉, C축을 돌려서 W축과 일치시킨다.  여기서 Rc는 W축 기준으로 C 좌표의 축 방향이다.

다음, W축을 돌리는 경우이다.  즉, W축을 돌려서 C축과 일치시킨다.  여기서 Rw는 C축 기준으로 W좌표의 축 방향이다.

많은 문헌에서 이 표현을 주로 사용한다.  이동과 회전의 선형변환을 하나의 식으로 표현해 보자.

% 두 좌표계 사이의 회전 행렬의 표현

% Coded by funMV. 12/2013

clear; clc;

% 두 좌표계 사이의 회전이 없을 경우

xw = [20,20,0]';  % coord of a point w.r.t world coord system

xt = [50 ,10, 100]';  % translational vector between world -> camera 

xc1 = xw - xt;

% 만일 C축이 W축에 대해 R만큼 회전된 경우

a2p=pi/180;

Rc_w = rot(20*a2p, 10*a2p, -30*a2p); % C축 3개의 방향(w축에 상대적으로)

Rw_c = Rc_w'; % W축 3개의 방향(c축에 상대적으로)

xc2 = Rw_c*(xw-xt); 

calibration된 물체를 화면에 표시하기

Opencv을 이용하면 Camera의 K, R, t 등을 구할 수 있다.

R, t를 이용해서 카메라축 기준으로 보정에 사용된 패턴을 3차원 공간에 뿌려보자.  보정에는 여러 영상을 사용한다.  다양한 자세의 카메라로 고정된 하나의 교정자(scale)를 활영한다.


캘리브레이션을 통해 R, t를 얻었다면 보정자의 좌표 M(w.r.t world coordinate: 3-vector)은 이미 알고 있다.  카메라를 기준으로 보정자 좌표 M'(camera coordinate)가 필요하다.

m = K[R|-Rc]M = KM'

이고

M'=[R|-Rc]M = inv(K)*m

이다.   단, c는 월드축을 기준으로 했을 때 translation 벡터이다.
주의할 점은 calibration에서 얻은 t는 -Rc이다. 따라서,

c=-R'*t

이다.

카메라의 위치를 기준으로 교정자를 표시 하였다. 잘 표시 되었다.

보정에서 얻은 R은 카메라 축의 방향을 나타낸다.
카메라 좌표의 축 3개의 방향(단위 벡터)이 각각 u, v, w라고 하면 R=[u';v';w']이다.

R은 어떤 월드 축에 대한 카메라 축의 방향이다. 카메라 축을 고정하면 월드 축은 R'로 돌려주면 된다.

이번에는 보정자를 고정시키고 영상을 찍은 카메라의 위치를 표시하면 다음과 같다.

(figures from JHKim)

World축과 Camera축 사이의 좌표 관계

평면 상에 한 점 P가 있다.

이 점은 world 좌표나 camera좌표를 기준으로 표시할 수 있다.

그림에서 camera좌표는 world축에 비해 반시계 방향으로 90도 회전되어 있다.  두 축에 대해 점의 좌표를 표시해 보자.

camera축의 방향은 camera 두 축의 단위 벡터 방향을 말한다.  또한 이 값이 camera축의 회전 행렬을 정의하게 된다.



World축과 Camera축 사이의 좌표 관계

World축과 Camera축을 일치 시키기 위해 두 좌표를 움직여 보자.

(1) camera축을 world축으로 평행이동 하자. 동차 좌표(homogeneous coord.)와 선형 변환(linear transform)으로 표현한다.

(2) 두 축의 자세를 일치 시키기 위해 camera축의 방향에서 나온 R을 연속해서 곱한다. 여기서 R은 카메라 축의 방향으로 R=[uc;vc;wc]이다.

이동과 회전의 두 선형변환 행열에 동차 좌표를 곱하면 변환 좌표 값을 얻을 수 있다.