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)

%} 

이미지의 affine 변형 함수

이미지의 affine 변형을 계산. 입력 프레임 img 내에서 p파라메터에 있는 affine계수에 따라 일정 부분을 취해 sz(예를 들면, 32x32)크기의 영상으로 만들어 리턴. p 행렬 내에 복수개의 affine 계수가 넘어가면 복수 개의 sz 크기 영상들이 생성되어 리턴.


function wimg = warpimg(img, p, sz)
% function wimg = warpimg(img, p, sz)
%
%    img(h,w)   :   입력 프레임
%    p(6,n)     :   Affine 인자 행렬(n은 affine 인자 set의 갯수)
%    sz(th,tw)  ：  입력 영상 내에서 취할 template 크기
%
%    이미지의 affine변형을 고속으로 처리하기 위한 함수

%% Copyright (C) 2005 Jongwoo Lim and David Ross.
%% All rights reserved.

if (nargin < 3)
    sz = size(img);
end
if (size(p,1) == 1) % size(param0,1)=1
    p = p(:); % param0는 행벡터인데 열벡터로 만들어 줌
end
w = sz(2); % 32
h = sz(1); % 32
n = size(p,2); % size(p,2)=1
[x,y] = meshgrid([1:w]-w/2, [1:h]-h/2);
% x=[ -15 ... 16;
%     -15 ... 16;
%        ...
%     -15 ... 16], size(x)=32x32
% y=[ -15 .... -15;
%     -14 ...  -14;
%         ...
%      16 ...   16]; size(y)=32x32
pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ...
              * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]);
% size(ones(h*w,1))=1024, size(x(:))=1024
% CAT Concatenate arrays.
%    CAT(DIM,A,B) concatenates the arrays A and B along
%    the dimension DIM.
%    CAT(2,A,B) is the same as [A,B].
%    CAT(1,A,B) is the same as [A;B].
% cat(2, ones(h*w,1),x(:),y(:)) =
%  [ 1 -15 -15; 1 -15 -14; 1 -15 -14; ...; 1 16 15; 1 16 16]: 1024x3
% [p(1,:) p(2,:); p(3:4,:) p(5:6,:)]
%  177.0000  147.0000
%    0.1166   -0.6368
%   -0.6368    3.4771
% p1=cat(2, ones(h*w,1),x(:),y(:))*[p(1,:) p(2,:); p(3:4,:) p(5:6,:)]
% size(p1)=1024x2
% size(pos)=32x32x1x2
%
% (Example) For one pixel and one set of affine params:
% (1 x y).(tx ty; a11 a21; a12 a22)=(tx+a11.x+a12.y   ty+a21.x+a22.y)
% For all pixels in sz (32x32=1024 pixels) and one set of affine parameters:
% [1 x1 y1; 1 x2 y2; ....].[tx ty; a11 a21; a12 a22]
%      = [tx+a11.x+a12.y   ty+a21.x+a22.y; tx+a11.x+a12.y   ty+a21.x+a22.y; ...]
% *size: (1024x3).(3x2) = (1024x2)
% For all pixels & n sets of affine parameters:
% [1 x1 y1; 1 x2 y2; ....].[tx(1)  tx(2)  ... tx(n)     ty(1)     ty(2)  ... ty(n);
%                                 a11(1) a11(2) ...a11(n)    a21(1) a21(2) ...a21(n);
%                                 a12(1) a12(2) ...a12(n)    a22(1) a22(2) ...a22(n)];
% * size: (1024x3).(3xnx2)=(1024xnx2) --> (32x32xnx2)

wimg = squeeze(interp2(img, pos(:,:,:,1), pos(:,:,:,2)));
% size(wimg)=32x32
% interp2(z,x1,y1): bilinear interpolation for the values of the
%    underlying 2-D function z at points in matrices x1 and y1.
                                %%pos(:,:,:,1)    :   x麟깃앤黎
                                %%pos(:,:,:,2)    ：  y麟깃앤黎
wimg(find(isnan(wimg))) = 0;    %%nan인 값을 찾아 0으로 만들어 줌
%ISNAN  True for Not-a-Number.
%    ISNAN(X) returns an array that contains 1's where
%    the elements of X are NaN's and 0's where they are not.
%    For example, ISNAN([pi NaN Inf -Inf]) is [0 1 0 0].

%   B = SQUEEZE(A) returns an array B with the same elements as
%   A but with all the singleton dimensions removed.  A singleton
%   is a dimension such that size(A,dim)==1;

Particle filter에 의한 영역 추적

연속 이미지에서 미리 설정해준 물체 영역(patch)을 particle filters에 의해 추적한다.  이 때, 파티클의 발생 방법에 대해 알아 본다.

먼저 사용자가 영상 내에서 물체 영역을 설정한다. 구체적으로 예를 들어 보면,

                                 (figure from [2])

-image size: 352(가로)x288(세로)
-red window 중심 위치, 크기:  (177(col_center) 147(row_center)), [115(col_length)x 145(row_length)]

(1) 처음 사용자가 설정해 준 영역은 추적 대상이 된다.  현재 설정 영역의 크기는 너무 크므로 가로,세로 폭을 줄여 32x32크기로 만든다. 이를 templates로 한다.

(2) 목표 물체는 3차원 운동을 할 수 있지만 이미지에 맺히는 2D 운동은 affine motion이라 가정한다. 물체의 2d운동을 표현하기 위해 geometric affine parameter를 설정한다.

% 추적 사각영역의 상태변수:

% col-center, row-center, 수직 스케일, 회전각 theta, 가로세로비, 각도 파이
% state=(tx, ty, s1, theta, s2/s1, phi)
geo_param = [177, 147, 115/32, 0, 145/115, 0]; % geometric affine 인자

% 가중치를 가진 particle들을 발생시켜 보자
n =  600;  % 샘플의 갯수는 600개로 한다.
N = 1024;  % 32x32 템플릿을 1차원 벡터로 표현하면 32x32=1024이다.

params = repmat(geo_param, [1,n]);  % geo affine param을 600개 복사. 6x600행렬 생성
randMatrix = randn(6,n); % 6x600개 정규 분포의 행렬 생성,
          % 정규 분포이니 평균은 0이다: mean(randMatrix,2)=(0.023 0.042 ...-0.036)

% affsig = [4 4 0.02 0.02 0.005 0.001]: 즉, center 위치 변화는 크다.
% 나머지 인자는 약간 변화한다 가정
params = params + randMatrix.*repmat(affsig(:),[1,n]); % 600개의 parameters생성

% 입력 영상을 600개의 affine 인자에 따라 변형 시킴. 즉, 입력 frame을 변형 시킨다.
% 출력은 32x32 크기의 템플릿 600개
wimgs = warpimg(frame, affparam2mat(params), [32,32]);

% template에서 600개 각각을 빼서 잔차 이미지를 구한다.
% template이미지는 기준영상으로 첫번째영상에서 물체
% 이미지, PCA영상, 모델 영상 등이다.
diff = repmat(template(:),[1,n]) - reshape(wimgs,[N,n]);

% 잔차 값을 이용하여 confidence를 계산
confidence = exp(-sum(diff.^2)./condenssig)'; %condenssig=0.25(잔차 분산 값)

% confidence를 확률 분포(pdf)로 만들기 위해 정규화한다.
confidence  = confidence ./ sum(confidence);

% 확률 값이 가장 큰 후보를 선택
[maxprob, maxidx] = max(confidence); % maxprob=0.2233(130번째)

% 이 샘플에 대한 affine para 값을 저장
affine_est = affparam2mat(params(:,maxidx)); %[a11 a12 tx; a21 a22 ty]

% 최적 이미지도 저장한다
timgs = [timgs wimgs(:,:,maxidx)];



참고 문헌

[1] D.A. Ross, J. Lim, et al., Incremental Learning for Robust Visual Tracking
[2] Dong Wang et. al., Online Object Tracking with sparse prototypes

Affine transformation

Affine 변환은 위치 이동과 선형변환(linear transformation)의 결합이다.

(x' y' 1)' = [a11 a12 tx; a21 a22 ty; 0 0 1].(x y 1)'

로 표현된다.  행렬로 더 축약해서 표현하면

x' = H.x = [A t; 0' 1].x

이다. 여기서 행렬 A는 2x2 크기의 비 특이(non-singular) 행렬이다.  평면상에서 일어나는 affine 변환은 6자유도를 가진다 (행렬 내에 6개의 미지수가 있다).  만일 3개의 대응점이 있다면 하나의 대응점이 2개의 식을 주므로 affine변환의 6개 인자 결정이 가능하다.

선형 변환 행렬 A는 2개의 기하 변환으로 설명할 수 있다.

(1) 회전
(2) 비 등방성(non-isotropic) 크기변화

아래 그림은 강체 회전과 shearing 변형의 2가지 기하 변환을 보여준다.

기하 변환의 값들은 2x2 크기의 A행렬을 특이값 분해하면 얻을 수 있다:

(예1) 파이 각 적용 여부에 따른 차이 점

>> clear all
>> fi=30*pi/180; % phi을 30도로
>> th=60*pi/180; % theta를 60도로
>>
>> Rf=[cos(fi) -sin(fi); sin(fi) cos(fi)];
>> Rt=[cos(th) -sin(th); sin(th) cos(th)];
>> D=[1.5 0; 0 1.0];
>>
>> A=Rt*Rf'*D*Rf
>> [u,d,v]=svd(A);
>> % d는 D이다. Rt==uv', Rf=v'이다.
>>
>> % 실제 변형을 테스트 해보자.
     % (아래 그림에서 원형점(정사각형))
>> pts=[1 1 -1 -1; 1 -1 -1 1];
>> pts =
     1     1    -1    -1
     1    -1    -1     1
>> D*pts =
    1.5000    1.5000   -1.5000   -1.5000
    1.0000   -1.0000   -1.0000    1.0000
    % shearing 변형 없이 스케일링만 일어남 (아래 그림에서 삼각점)

>> Rf'*D*Rf*pts
    1.1585    1.5915   -1.1585   -1.5915
    0.9085   -1.3415   -0.9085    1.3415
    % shearing 변형이 일어남 (아래 그림에서 사각점)

(예2) tracking에 적용하기 위한 geometric parameter로 변환.

영상에서 물체를 추적할 때 보통 물체 영역을 둘러싸는 작은 사각의 윈도(window patch)를 가정한다.
물체가 크기 변화, 회전, 이동 할 때, 이에 따라 윈도 영역도 크기변화, 회전, 이동해야 한다. 이러한 기하학적 변화를 나타내는 parameter를 affine 변환에서 유도한다.

즉, 일반적으로 알고 있는 affine 변환의 6개의 parameter를 geometric affine 인자로 변환하는 matlab코드는 아래와 같다. Geometric parameter 6개는
(x, y, scale, th, aspect, skew) 이다.
여기서, (x,y)는 윈도의 중심 위치, scale은 수직방향 스케일, th는 회전 각, aspect는 수직-수평 비, skew는 파이 각을 나타낸다.

이러한 변환을 통해 추적되는 물체의 상태 변수(state variable)는 물체를 둘러싸는 윈도의 상태를 나타내는 6개의 geometric parameter가 된다.

function q = affparam2geom(p)
% function q = affparam2geom(p)
%
%   입력  ： p，2x3의 affine 계수；
%   출력  ： q，geometric affine 계수로 각도 스케일비 등을 나타냄；
% The functions affparam2geom and affparam2mat convert a 'geometric'
% affine parameter to/from a matrix form (2x3 matrix).
%
% affparam2geom converts a 2x3 matrix to 6 affine parameters
% (x, y, scale, th, aspect, skew), and affparam2mat does the inverse.
%
%    p(6) : [p(1) p(3) p(4); p(2) p(5) p(6)]
%
%           x'              p(3)   p(4)   p(1)           x
%           y'      =       p(5)   p(6)   p(2)     *     y
%           1                 0        0        1            1
%
%                           p(3)   p(4)
%           A       =      
%                           p(5)   p(6)
%
%    q(6) : [dx dy sc th sr phi]
%           dx,dy   :   left,top의 원점을 기준으로 box의 중심 위치 표현
%           sc,sr   :   ramda1(수직방향으로 스케일), ramda2/ramda1 (길이 비율)
%                       수직 방향 스케일은 box크기가 32이므로 32의 배수로 주어짐
%           th      ：  각도 theta
%           phi     ：  각도 phi
%
% Reference "Multiple View Geometry in Computer Vision" by Richard
% Hartley and Andrew Zisserman.
% 출력은 [tx, ty, ramda1, theta, ramda2/ramda1, phi]
% MVG 책의 39~40페이지에 설명

% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.

A = [ p(3), p(4); p(5), p(6) ]; % A=[3.594 0; 0 4.531]: y(col)크기가 더 큼
%%  A = USV' = (UV')(VSV') = R(th)R(-phi)SR(phi)

[U,S,V] = svd(A);
% (Ex.) 축의 순서 결정. For example,
% S=[4.53 0; 0 3.59]; U=[0 1; 1 0];
% 영상내 설정은 x축(col), y축(row) 순서임. Row가 더 크니
% U가 (0 1)', (1 0)' 벡터의 순서로 나옴(즉, y축이 먼저 나옴)
% 따라서, x축이 우선되게 축 순서 조정 필요

if (det(U) < 0) % 항상 x축이 우선되게 하기 위함
  U = U(:,2:-1:1);  V = V(:,2:-1:1); % 2열을 1열로
  S = S(2:-1:1,2:-1:1); % 2행을 1행으로, 2열을 1열로
  % U=[1 0; 0 1], S=[3.59 0; 0 4.53]로 수정 됨
end

%%*******중심 위치*******%%
q(1) = p(1);
q(2) = p(2);
%%*******(col, row)*******%%

% [cos(th) -sin(th); sin(th) cos(th)]=uv'
%   =[u11 u12; u21 u22][v11 v21; v12 v22]
%   =[u11.v11+u12.v21 ...; u21.v11+u22.v12 ...]-->
q(4) = atan2(U(2,1)*V(1,1)+U(2,2)*V(1,2), U(1,1)*V(1,1)+U(1,2)*V(1,2)); % theta


phi = atan2(V(1,2),V(1,1)); % angle phi
if (phi <= -pi/2) % phi가 -90도 이하면 그냥 -90도로
  c = cos(-pi/2); s = sin(-pi/2);
  R = [c -s; s c];  V = V * R;  S = R'*S*R;
end
if (phi >= pi/2) % phi가 90도 이상이면 그냥 90도로 둠
  c = cos(pi/2); s = sin(pi/2);
  R = [c -s; s c];  V = V * R;  S = R'*S*R;
end

%%*******크기비율*******%%
q(3) = S(1,1); % ramda1
q(5) = S(2,2)/S(1,1); % ramda2/ramda1
%%*******각도파이*******%%
q(6) = atan2(V(1,2),V(1,1)); % phi

q = reshape(q, size(p));
% 출력은 [tx, ty, ramda1, theta, ramda2/ramda1, phi]




(예3) Geometric affine parameters를 2x3크기의 affine 행렬로 변환하는 코드

function q = affparam2mat(p)
% function q = affparam2mat(p)
%
% The functions affparam2geom and affparam2mat convert a 'geometric'
% affine parameter to/from a matrix form (2x3 matrix).
%
%   입력  ： q，기하학적 affine 계수；
%   출력  ： p，2x3행렬에서의 affine 계수；
%
% affparam2geom converts a 2x3 matrix to 6 affine parameters
% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse.
%
%    p(6,n) : [dx dy sc th sr phi]'
%    q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)]
%
% Reference "Multiple View Geometry in Computer Vision" by Richard
% Hartley and Andrew Zisserman.
%
% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.


sz = size(p); % size(p)=1x6
if (length(p(:)) == 6) % p(:)= (177.0000; 147.0000; 3.5938; 0; 1.2609; 0)
  p = p(:);            % length(p(:))=6
                       % p = (177.0000; 147.0000; 3.5938; 0; 1.2609; 0)
end
s = p(3,:);  th = p(4,:);  r = p(5,:);  phi = p(6,:);
cth = cos(th);  sth = sin(th);  cph = cos(phi);  sph = sin(phi);
ccc = cth.*cph.*cph;  ccs = cth.*cph.*sph;  css = cth.*sph.*sph;
scc = sth.*cph.*cph;  scs = sth.*cph.*sph;  sss = sth.*sph.*sph;
q(1,:) = p(1,:);  q(2,:) = p(2,:);
q(3,:) = s.*(ccc +scs +r.*(css -scs));  q(4,:) = s.*(r.*(ccs -scc) -ccs -sss);
q(5,:) = s.*(scc -ccs +r.*(ccs +sss));  q(6,:) = s.*(r.*(ccc +scs) -scs +css);
q = reshape(q, sz);





참고 문헌

[1] Hartley and Zisserman, Multiple View Geometry, Cambridge press