Particle filter 소개

물체 추적은 은닉 변수를 가진 Markov모델에 Bayesian 추론을 적용하는 것이다$^{주1}$. 시간 t 프레임에서 관찰 이미지(observation) 집합 $Y_t$={y1,y2,...,yt}가 주어진다고 할 때, 은닉 상태 변수 $x_t$는 recursive하게 추정할수 있다. 

여기서, p(xt|xt-1)은 두 연속 상태 사이의 시스템 동적 모델이다. p(yt|xt)는 상태 xt에서 관찰(observation) 모델의 우도(likelihood)를 나타낸다.

이 식의 우변에서 시간 인덱스 $t-1$을 $t$로($t$는 $t+1$로) 바꾸고 좌변의 $p(x_t|Y_t)$를 다시 대입하면 시간 인덱스만 진전된 동일한 표현이 되므로 위 식은 반복해서 적용 가능한 재귀적(recursive) 표현이다.


Particle filter로 위 식을 표현하면, t프레임까지의 모든 관찰값이 주어질 때 추적 물체의 최적 상태는, 시간 t에서 N개의 샘플에서의 최대 사후확률을 추정하는 것이다.

이 식은 재귀적 형태를 포함하고 있다. $p(x_t^i|x_{t-1})$에 따라 $t-1$시간까지 추정한 상태 값 $x_{t-1}$의 근방에서 $x_t^i$를 확률적으로 발생시키고 각각의 파티클 $x_t^i$에서 관찰 값  $p(y_t^i|x_t^i)$를 얻는다.  시간 $t$에서의 새로운 상태는 두 확률 곱의 최대 값을 주는 $x_t^i$가 된다.


  

Particle filter의 simulation 예
아래 코드는 1차원 운동을 하는 물체를 tracking하는 pf를 보여준다.

움직이는 물체의 운동은 2개의 sine wave와 하나의 linear motion형태의 궤적이 결합되어 있으며 결합부에 모션 불연속부가 존재한다.
불연속이 있어도 pf는 물체를 놓치지 않고 성공적으로 대상을 추적하는 것을 보여준다.

-Green dots: particles (150개)

-Red line: true state

-Blue line: estimated state

-모션 불연속(red line)이 있는 위치가 2군데 있음(위치 데이터는 크게 삼등분 가능)

-특정 위치에서의 거동 설명을 위해 13frame, 53frame 위치가 blue 색의 수직 라인으로 표시되고, 아래 그림에서 설명.

위 그림은 100프레임 동안 물체를 추적한 결과 이다. 수직 위치를 추적하는 것이며, 약 25프레임 근방과 49프레임 근방에서 모션 불연속이 존재한다.
particle들은 운동물체의 궤적을 잘 따라 가고 있다.

13프레임의 푸른색 수직선을 보면 물체의 궤적이 연속곡선으로 부드럽게 이동한다. 따라서, particle들은 mean주위에 집중되어 분포한다.
추정된 현재 위치(녹색점) 근방에서 측정치(measurement) 값도 크므로 큰 변화없이 부드러운 추적이 발생한다.

53프레임의 수직 선 위치에서는 직전 49프레임에서의 갑작스러운 불연속 모션으로 인해 파티클들은 수직 위치 -2 근방에 주로 놓여 있고 물체의 현재 궤적은 1 근방이다.  측정치의 pdf를 보면 1 근방에서 확률값이 크므로 이 주변의 파티클들의 가중치가 크지므로 곧 -2 근방에 있던 파티클들이 1 근방으로 모이게 된다.


%
% 1D particle filter tracking a height position
% Summarized by funMV, 12/2013 
% [Ref.] T. Svoboda, J. Kybic, V. Hlavac, Image processing, Analysis, 
%   and Machine Vision, A Matlab companion, Thomson press.
%   pp.232
%   
%   Original version is in http://visionbook.felk.cvut.cz
%
clear all;
clear classes;

% 파라메터 설정
conf.steps = 100; % number of steps(100프레임 동안)
conf.sigma_1 = 0.02; % standard deviation - system
conf.N = 150; % number of samples(파티클)
conf.sigma_2 = 0.5; % standard deviation for the measurement function
conf.Cov = 0.1; % covariance matrix - sampling (scalar for 1D)
conf.alpha = 0.3; % factor for exponential forgetting - motion model


% tracking할 대상 데이터의 생성(true values)
% 크게 3부분의 데이터 생성(첨부 자료 참조)
third = round(conf.steps/4);
X(1) = normrnd(0,conf.sigma_1);
for i = 1:(third-1)
  X(i+1) = X(i) + (sin(2*pi*i/third)-sin(2*pi*(i-1)/third)) + ...
           normrnd(0,conf.sigma_1);
end
X(third) = sin(3*pi/2) + normrnd(0,conf.sigma_1);
for i = third:(2*third-1)
  X(i+1) = X(i) + (sin(2*pi*i/third+3*pi/2)-sin(2*pi*(i-1)/third+3*pi/2)) + ...
           normrnd(0,conf.sigma_1);
end
increment = -2/(conf.steps-2*third);
X(2*third) = 1 + normrnd(0,conf.sigma_1);
for i = (2*third+1):conf.steps
  X(i) = X(i-1) + increment + normrnd(0,conf.sigma_1);
end



% 파티클 값들의 설정
%
S.s   = normrnd( 0, conf.Cov, 1, conf.N ); % initialize samples
% 평균을 0, 분산을 conf.Cov로 한 정규분포의 샘플을 conf.N개 발생
% S.s=1x150, mean(S.s)=-0.0011

S.pi  = ones(1,conf.N) / conf.N;   % set uniform weights
% 처음에는 가중치를 전부 1/N의 값을 가지게 함. size(S.pi)=1x150

S.v   = 0;                                 % initialize motion model
S.est = estimation( S, conf );    % set estimated value
% S.pi*S.s' = -0.001(평균이 0에서 샘플이 발생 했고,
%           모든 가중치는 동일하므로 기대값(가중평균)은 0)


data =[];

% tracking의 실행
for step = 2:conf.steps           % for all generated states
  S.x = X(step);                  % states of the system. 추적할 실제 값
  S = particle_filtering( S, conf ); % run particle filtering

  % 결과 확인을 위한 데이터 저장
  data = [data; step X(step) S.est abs(S.est-X(step))]; % real(true), estimation, error
end





function S = particle_filtering(S,conf)
% PARTICLE_FILTERING Particle filtering (Condensation)
% CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Petr Lhotsky, Tomas Svoboda, 2007
%
% Usage: S = particle_filtering(S,conf)
% Inputs:
%   S  struct  Structure with samples and weights. 
%   .x [dim x 1]  State of the system. This represent the true
%     state of the system. It is used for demonstration purposes
%     only. It is normally unknown. dim denotes dimensionality of
%     the state space.
%   .s    [dim x N]  Matrix containing N samples (particles).
%   .pi   [1 x N]    Weights of the samples.
%   .v    [dim x 1]  Predicted velocity - motion model.
%   .est  [dim x 1]  Estimated value - estimated state of the observed system.
%   conf  struct     Structure with configuration.
%   .N    1x1        Number of samples.
%   .Cov  [dim x dim]Noise covariance matrix.
%   .alpha  1x1      Factor for the exponential forgetting of the
%     motion model; 
%     lower values put the accent on the history and 
%     higher values on the actual movement; 0 turns the motion model off.[-.75ex]
% Outputs:
%   S  struct  The same structure as for input, but at time step k+1.
%
% The filtering procedure consists of the following steps:
% Resample data S_ using importance sampling. The sampling
% procedure draws samples from the set such that samples with higher
% weights are likely to be picked more times. Since the total
% number of samples is preserved, some samples with lower weights might
% not be
% selected at all. Note that we are still using the states at time t-1.
% The importance sampling step is performed by
% function importance_sampling.
S.s = importance_sampling( S.s, S.pi ); % S.s: 정규분포(0-mean), S.pi(weight, 1/N)

% Predict the state for time t, and add noise.
% Prediction is here only a simple motion drift, see drift_samples.
% See function noisify for details of adding noise.
S.s = drift_samples( S, conf );
S.s = noisify( S, conf );

% The correction (measurement) step evaluates the likelihood of how well the samples
% This function depends on the application, see a simple
% measurement1D or a more realistic measurement2D.
S.pi = measurement( S, conf );

% normalize weights
S.pi = S.pi ./ sum(S.pi);

% Compute a pointwise state estimate from the samples. It is worth mentioning
% that the computation of the state estimate may be meaningless in
% some situations, especially for
% multimodal distributions.
S.est_old = S.est;
S.est = estimation( S, conf );

% Update the state velocity which is needed for the simple
% drift. Simple exponential forgetting is applied. The velocity
% does not need to be used in some applications.
S.v = conf.alpha*(S.est-S.est_old) + (1-conf.alpha)*S.v;

return; % end of particle filtering




function s_noise = noisify(S,conf)
%NOISIFY Noisify the samples with normal noise
%CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Usage: s_noise = noisify(S,conf)
% Inputs:
%   S  struct  Structure with samples and weights.
%   conf  struct  Structure with configuration.
% Outputs:
%   s_noise  [1 x N]  Vector with samples.
% Add noise to samples for a given covariance matrix conf.Cov.
% This implementation is based on .
%
%
% Based on:
% About: Statistical Pattern Recognition Toolbox
% (C) 1999-2003, Written by Vojtech Franc and Vaclav Hlavac
% <a href="http://www.cvut.cz">Czech Technical University Prague</a>
% <a href="http://www.feld.cvut.cz">Faculty of Electrical Engineering</a>
% <a href="http://cmp.felk.cvut.cz">Center for Machine Perception</a>

[dim N] = size( S.s );   % get dimension
[U,L] = eig( conf.Cov ); % compute eigenvalues and eigenvectors
s_noise = S.s + inv(U')*sqrt(L)*randn(dim,N); % dewhitening transform
% S.s를 빼고 생각해 보면, Cov행렬을 고유값분해 해서 고유벡터 U와 그 기저에서의
% 좌표 값 L을 얻었다. 좌표 값 L은 Cov행렬의 크기이다. 여기에 곱해 평균 0과 분산 1을
% 가진 정규분포에서 샘플을 N개 생성한다면 평균 0근방에서 분산 L을 가진 분포의
% 샘플을 얻는 것이다.
% U'.a는 U기저로 어떤 벡터 a를 표현하는 것이다. U'a는 U기저에서의 벡터 a의 좌표
% 값이다. 따라서,
% U'a = sqrt(L)*randn(dim,N)에서 sqrt(L)*randn(dim,N)는 U기저에서의
% a벡터의 좌표 값이다.
% a = inv(U')*sqrt(L)*randn(dim,N)는 표준 기저에서의 좌표벡터를 얻는 것이다.
% (원래 상태 벡터의 값들은 표준 기저에서의 값들이었다)
% 정리하면, 샘플 분포는 기저 U에 대한 정규분포를 갖도록 발생시키고
% 표준 기저의 벡터로 다시 변환해 주는 것이다.
return





function [w] = measurement(S,conf)
%MEASUREMENT Evaluation of the samples
%CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Usage: w = measurement(S,conf)
%  S  struct  Structure with samples and weights. 
%  conf  struct  Structure with configuration. 
%   .sigma_2  [1]  Standard deviation of the noise.
%  w  [1 x N] Vector with measurements for each sample. 
% Function measurement evaluates each sample according to a measurement
% function and returns a likelihood of each sample.
% The measurement function is modeled by a Gaussian centered around the 
% true value and a small constant is added to represent a non-zero
% response of a real measurement function.
% This is an approximation of an ideal which should be smooth and
% peak at the true value. The non-zero observation likelihood further away from the true state
% also prevents samples from dying prematurely. 
% See demo2D/measurement for a more
% realistic measurement function.
%

w = normpdf((S.s-S.x),0,conf.sigma_2) + 0.1;
return




function [x_res,freq] = importance_sampling(x,p,varargin)
% IMPORTANCE_SAMPLING importance sampling
% CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Tomas Svoboda, 2007
%
% Usage: [x_res,freq] = importance_sampling(x,p,Nnew,show,fid,save)
% Inputs:
%   x  [dim x N]  Matrix of N samples of dimension .
%   p  [1 x N]  Probabilities of samples.
%   Nnew  (default N)  Number of new samples.
%   show  (default 0)  If set to 1 the sampling steps are
%     graphically visualized. It works only for 1D data.
%   fid   (default gcf)  Figure handle for the graphical visualization.
%   save  (default 0)  Save the graphically visualized steps to disk. 
% Outputs:
%   x_res  [dim x Nnew]  Selected (sampled) samples from x.
%   freq  [1 x N]  Frequencies of selections, giving the number of
%     times a particular sample from x was selected.

Nnew = length(x);

cum_prob = cumsum(p);    % cumulative probability, cum_sum=1x150
x_res = zeros( size(x,1), Nnew); % space for resampled samples, x_res=1x150(전부 0)
freq = zeros( size(x) );    % selection frequency of the old samples, freq=1x150, 전부 0


for i = 1:Nnew
  % selection sample from a uniform density 0...1
  r = rand; % 0과 1사이의 난수 발생
  j = find( cum_prob==min(cum_prob(cum_prob>r)), 1 );
  % cum_prob>r이 되는 cum_prob요소중 최소 값을 주는 index계산
  % take the j-sample to the new sample set
  x_res(:,i) = x(:,j); % j인덱스에 있는 값을 저장
  % x_res에는 더 많이 샘플링된 j인덱스가 주는 x값이 더 여러번 저장됨.
  % increment the frequency table
  freq(j) = freq(j) + 1; % j인덱스의 count를 증가
end
return; % end of importance_sampling, x_res만 리턴됨.




function est = estimation(S,conf);
%ESTIMATION The best guess for the state of the system - estimated value
%CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Usage: est = estimation(S,conf)
%  S  struct  Structure with samples and weights. 
%  conf  struct  Structure with configuration. 
%  est  [dim x 1] Value of the predicted state. 
% Computes the best guess for the state of the system 
% as the weighted average of the samples 
% x = sum_i^N,
% which is implemented as a scalar product of vectors 
%  and s.
%

est = [S.pi]*[S.s]';
% 평균을 구한다고 보면 됨. 샘플이 S.s에 들어 있고, 각 샘플의 가중치가 S.pi
% 이므로 각 샘플들의 가중 합이 계산. 즉, 기대값, 평균임.
return % end of estimation



function s_drift = drift_samples(S,conf)
%DRIFT_SAMPLES Drift samples according to the velocity of the motion model
%CMP Vision Algorithms http://visionbook.felk.cvut.cz
% Usage: s_drift = drift_samples(S,conf)
%  S  struct  Structure with samples and weights. 
%  conf  struct  Structure with configuration. 
%  s_drift  [dim x N] Drifted samples. 
% Drift samples according to the velocity model. 
% This is a simple demonstration function.
% A more general predictor is naturally possible.
%

s_drift = S.s + S.v;
return

(주1) 은닉변수(hidden variable)는 시스템에 숨겨져 있는 상태 변수(vector) $x$를 의미한다. 즉 관찰 값 $Y$를 이용하여 숨겨진 $x$를 추정하는 문제이다.  Markov 모델이란 측정 값이 서로 독립적이지 않은 연속 값의 체인으로 발생한다는 것이다.  서로 인접한 값은 상관성이 크고 멀어질 수록 상관성이 작아진다.  Bayesian 추론이란 수식 자체가 Bayesian theory를 이용하여 얻어지기 때문에 이렇게 표현한다.

Two dimensional interpolation

% Solving peak position for 2d grid values
%   by 2nd order polynomial surface fitting
% Coded by funMV. 12/2013
%
% v(x,y)=v(row,col)=a1+a2*x+a3*y+a4*x*y+a5*x^2+a6*y^2
% parameters ai is solved by linear equation system:
% v(-1,-1)=a1-a2-a3+a4+a5+a6
% v(-1, 0)=a1-a2+a5
% v(-1, 1)=a1-a2+a3-a4+a5+a6
% v( 0,-1)=a1-a3+a6
% v( 0, 0)=a1
% v( 0, 1)=a1+a3+a6
% v( 1,-1)=a1+a2-a3-a4+a5+a6
% v( 1, 0)=a1+a2+a5
% v( 1, 1)=a1+a2+a3+a4+a5+a6 
%
% Max value is at extreme position:
%   dv(x,y)/dx = 0
%   dv(x,y)/dy = 0
%
%Two dimensional interpolation for finding max value on sub-pixel. Given values are on %grid and 2nd order polynomial surface is fit to grid values.

clear all; clc;

A=[1 -1 -1  1  1  1;
   1 -1  0  0  1  0;
   1 -1  1 -1  1  1;
   1  0 -1  0  0  1;
   1  0  0  0  0  0;
   1  0  1  0  0  1;
   1  1 -1 -1  1  1;
   1  1  0  0  1  0;
   1  1  1  1  1  1];
pinv = inv(A'*A)*A';  % 미리 계산해 둘 수 있음

b=[21 22 21   24 25 24   21 22 21]'; % 9개 격자 값. (중앙 값이 최대)
x= pinv*b;

%row = -1; col = 1;
%c1 = [1 row col row*col row*row col*col];
%val = c1*x;

m1 = [2*x(5) x(4); x(4) 2*x(6)];
b1 = [x(2); x(3)];

c2 = -inv(m1)*b1;
c2




References
[1] 김성완, 김남식, Digital Image Correlation 기법을 이용한 구조물의 다중 동적변위 응답측정, 한국지진공학회 논문집, 13권 3호, 2009.

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(구면 선형 보간)

Depth from known motion

% 공간 상의 한 점이 운동 전 후의 카메라에 투영되고
% 카메라의 "운동을 안다고 했을 때" 
% 공간 점의 depth를 확률적으로 추정하는 방법
% Coded by funMV. 12/2013

clear; clc;

% 1st camera
X1 = [2 4 50 1]'; % 공간 점
P1 = [eye(3) [0 0 0]']; % projection matrix(before motion)
u1 = P1*X1; % image coordinate
n1 = u1/norm(u1); % ray direction
p1 = linspace(1,100); % 100 particles (깊이 값을 임의로 100개 발생)
d1 = [p1*n1(1); p1*n1(2); p1*n1(3)];  % 100 particle vectors (이 중 하나는 진짜에 가까움)
% scale이 다른 공간 점의 homogeneous 좌표를 100개 발생 시킴 
d1 = [d1; repmat(1,[1,size(d1,2)])]; % homogeneous particle vectors

% 2nd camera
P2 = [eye(3) [3 0 0]']; % projection matrix(after motion)
u2 = P2*X1; % X1점을 두번째 카메라에 투영
s2 = [u2(1)/u2(3); u2(2)/u2(3)]; % [0.1; 0.08]


% projection on 2nd camera of 1st ray points
a2 = P2*d1; % 공간 점의 후보 100개 점을 두번째 카메라에 투영
m2 = [a2(1,:)./a2(3,:); a2(2,:)./a2(3,:)];
min_index = find( abs(m2(1,:)-s2(1)) < 0.0005); % 50

df = abs(m2(1,:)-s2(1));
max_index = find( max(df) );
max_value = max(df);
mn = mean(df);
sd = std(df); % standard deviation
vr = var(df); % variance

% 잔차 값을 이용하여 confidence를 계산
confidence = exp(-df./(0.1*vr))';
% 0.1*vr: 분모의 값을 더 작게하면 분산이 더 작으므로 최대 값 주위로 더 집중되는
% sharp한 분포의 confidence값이 나옴

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



정리하면 다음과 같다:

1. 카메라 운동 전, 운동 후(직진 운동이라 합시다)의 두 영상에서 LM점(sift나 surf 등)들을 이용하여 SFM을 수행하고 각 LM의 3차원 좌표 값, 카메라 운동에 대한 R, t 값을 결정한다.

2. 첫 번째 카메라(운동 전)에 맺힌 어떤 LM점의 좌표를 x1이라고 하자.  현재 우리가 x1점에 대해 모르는 것은 x1점의 scale뿐이다.  x1을 정규벡터(unit vector)로 만들자.

3. x1에 가능한 스케일의 후보 값을 100개를 곱해 scale만 다른 x1의 후보 값 100개 생성한다.  이 중 하나는 실제 x1(스케일까지 정확한 x1)값에 가까울 것이다.

4. 단계 3에서 생성한 100개의 점을 두 번째 카메라(운동 후)에 모두 투영시키자.   이것은 계산 가능하다. 왜냐하면 첫 번째 카메라와 상대적인 두 번째 카메라의 R, t를 모두 알고 있기 때문이다.

5. x1의 실제 공간 점이 두 번째 카메라에서 측정된 점의 좌표(두 번째 카메라에서 측정 가능)와  단계 4에서 100개의 점을 투영한 점 사이의 오차(error)에 따라 confidence를 정의할 수 있다.  이 confidence값이 100개의 particle의 확률 값이 된다.

6. 이 과정을 영상 내의 다른 LM에 대해 반복 수행한다.  또는 카메라를 계속 움직이면서 반복 수행한다.



참고 문헌
[1] A. J. Davison, Real-time simultaneous localization and mapping with a single camera, on ICCV.

함수를 통한 가우시안 분포의 전파

어떤 랜덤 변수 $x$가 Gaussian이라 가정한다:

$x \sim N(\mu_x, \sigma_x^2)$.


이 때 $y=f(x)$라 하자. 변수 $y$도 가우시안으로 근사될 수 있다면

$y \sim N(\mu_y, \sigma_y^2)$

여기서,

$\mu_y = f(\mu_x)$

$\sigma_y^2=\begin{pmatrix}{df \over dx}\end{pmatrix}_{\mu_x}\sigma_x^2 \begin{pmatrix}{df \over dx}\end{pmatrix}_{\mu_x}$

이다.

위 식에서 만일 $f$가 $\mu_x$ 주위에서 선형이라면 $\left(df \over dx \right)$는 상수이고, $y$는 가우시안이다. 함수 $y$가 선형이되는 간격의 크기는 분포 $\sigma_x$에 달려 잇다. $\sigma_x$가 더 커지면 랜덤 변수 $x$의 넓은 범위를 포함하기 위해 간격 크기는 더 커져야 한다.

간격 크기가 랜덤 변수 $x$분포의 95%를 커버하게 한다면 [$\mu_x-2\sigma_x, \mu_x+2\sigma_x$]이다.  만일 함수 $f$가 이 간격 내에서 선형(1차 함수)이라면, 도함수는 이 간격 내에서 상수이다.   간격 주위에서 도함수를 분석하기 위해 1차 도함수에 대한 Taylor시리즈를 생각해 보면

${df \over dx}(\mu_x+\delta x) \approx\left.{df \over dx}\right|_{\mu_x}+\left.{d^2 f \over dx^2}\right|_{\mu_x} \delta x$

이다.
중심위치 $\mu_x$에서 1차 도함수의 값은 $\left.df \over dx\right|_{\mu_x}$이고,
간격의 양 극단 $\mu_x \pm 2\sigma_x$에서 도함수 값은

$\left.df \over dx\right|_{\mu_x} \pm \left.d^2 f \over dx^2\right|_{\mu_x}  2\sigma_x$

이다.
선형화의 정도를 나타내는 인덱스 값을 정의하자.

$L=\left|{ \left.{d^2 f \over dx^2}\right|_{\mu_x}  2\sigma_x \over  \left.{df \over dx}\right|_{\mu_x}}\right|$

분자, 분모 두 값의 비교는 무차원인 선형화 인덱스(linearity index)값으로 정의한다. 만일 $L\approx 0$이라면(즉, $f$의 1차 도함수가 상수라면 $L$의 분자에 있는 2차 도함수는 0이다) 이 함수는 간격 내에서 선형으로 생각할 수 있고, 함수를 통한 가우시안 성질은 보존된다.




References

[1] J. Civera, AJ Davison, Inverse depth to depth conversion for monocular slam, ieee icra'2007.
[2] Error propagation 요약 자료
[3] 발표 자료

Design pattern - Factory pattern

서로 관련된 객체들의 집단을 생성하는 인터페이스를 제공한다.  생성되는 객체의 구체적 내용에 독립적인 개발을 가능하게 한다.


// 애완 동물 정의 클래스
class Animal
{
public:
Animal() {}
~Animal() {}
virtual void Speak()=0;
};

class Dog: public Animal
{
public:
Dog() 
{
cout << "개가 태어 났습니다. " << endl;
}
~Dog()
{
cout << "개가 죽었습니다. " << endl;
}
void Speak()
{
cout << "멍멍..." << endl;
}
};

class Cat: public Animal
{
public:
Cat() 
{
cout << "고양이가 태어 났습니다. " << endl;
}
~Cat()
{
cout << "고양이가 죽었습니다. " << endl;
}
void Speak()
{
cout << "야옹..." << endl;
}
};




// 동물 농장 클래스
class Farm
{
public:
Farm() { cout << "농장 오픈. " << endl; NewAnimal(); }
~Farm() { cout << "농장 폐쇄. " << endl; }
void NewAnimal()
{
Animal *pAnimal = CreateAnimal();
pAnimal->Speak();
}
virtual Animal* CreateAnimal()=0;
};

class DogFarm: public Farm
{
public:
DogFarm() { cout << "개농장 오픈. " << endl; }
~DogFarm() { cout << "개농장 폐쇄. " << endl; }
Animal* CreateAnimal()
{
return new Dog(); 
}
};

class CatFarm: public Farm
{
public:
CatFarm() { cout << "고양이 농장 오픈. " << endl; }
~CatFarm() { cout << "고양이 농장 폐쇄. " << endl; }
Animal* CreateAnimal()
{
return new Cat(); 
}
};

void main()
{
// 팩토리 패턴
DogFarm _dogFarm;
_dogFarm.NewAnimal();
}

Design pattern - Sterategy pattern

새로운 방식의 행위, 기능을 추가하거나 기존 방법을 변경할 때 편리하다.  전략패턴 내에서는 행위, 기능이 상위의 인터페이스 클래스를 통해 관리된다.   따라서 전략패턴을 사용하는 클라이언트는 전략패턴 인터페이스의 가상함수를 통해 객체의 구체적 행위에 접근한다.

인터페이스를 통한 연결을 이용하므로 기존의 행위를 변경하거나, 새로운 행위를 추가, 삭제할 때, 클라이언트나 추상 클래스의 변경없이 행위 클래스만 바꾸므로 유지 및 보수가 편리하고 모듈간 독립성을 유지할 수 있다.


전략 패턴의 표기에 일반적으로 사용되는 UML표기는 여기를 참고한다.

위 그림의 출처는 여기를 참고한다.  그림을 보면 두 추상 클래스는 불변이고, 하위의 자식 클래스만 수정된다.



전략 패턴 사용을 위해서는 두 클래스의 그룹을 만들어야 한다.


(1) 행위를 정의하는 그룹: Interface(JAVA), 또는 추상클래스(C++)를 만든다. 이것을 부모로 해서 상속받은 구체적 행위를 정의하는 class가 다수 존재하게 한다.

(2) 행위를 사용하는 그룹: 클라이언트(Client)라고도 부르고 (1)의 행위를 정의하는 추상 클래스 객체를 멤버로 포함(association)한다.  Client class를 상속받은 다수의 class를 다시 정의하며, 이 클래스들이 행위를 가지거나 사용하는 주체가 된다.  각 주체가 어떤 행위를 선택할지는 Client class내의 setter함수를 사용한다.

 
정리해서 생각해 보면, client class를 중심으로 행위 변경은 행위 그룹의 하단(child)에서, 행위를 사용하는 주체는 client class의 하단(child)에서 추가, 삭제, 수정된다.





(예1) 오리의 종류에 따른 행위 표현
#include <cstdlib>
#include <iostream>

using namespace std;


// 인터페이스 클래스의 정의
// Fly라는 행위에 대한 인터페이스
class FlyBehavior
{
public:
  virtual void fly() {};
};

// Fly 행위에 대한 구체적 행위를 나타내는 클래스
class Fly_Normal: public FlyBehavior
{
public:
  void fly()
  {
    cout << "오리날기 기본 - 훨훨!!!" << endl;
  }
};

class Fly_None: public FlyBehavior
{
public:
   void fly()
   {
     cout << "날 수 없음" << endl;
   }
};


// 오리 울음 행위를 나타내는 인터페이스 클래스
class QuackBehavior
{
public:
   virtual void quack() {};
};

// 오리 울음의 종류별 구체적 구현을 나타내는 클래스
class Quack_Normal: public QuackBehavior
{
public:
   void quack()
   {
    cout << "오리 울음 기본 - 꽥꽥!!!" << endl;
   }
};

class Quack_Beep: public QuackBehavior
{
public:
   void quack()
   {
    cout << "오리 울음 삑삑이 - 삑삑!!!" << endl;
   }
};



// Client class
// 행위를 사용하는 클래스
class Duck
{
private:
   QuackBehavior *quackBehavior; // 울음 정의 인터페이스
   FlyBehavior *flyBehavior; // 나는 모습을 정의하는 인터페이스

   void deletQuack() // quack behavior를 해제하는 함수
   {
    if(quackBehavior !=0)
    {
     cout << "이전에 설정된 quack behavior가 있다, 메모리 해제" << endl;
     delete quackBehavior;
    }
 }

 void deleteFly() // Fly behavior를 해제하는 함수
 {
    if(flyBehavior != 0)
    {
     cout << "이전에 설정된 fly behavior가 있다. 메로리 해제" << endl;
    }
 }


public:
   Duck()
   {
      quackBehavior = 0;
      flyBehavior = 0;
      cout << "오리 인스턴스 생성" << endl;
   }
  ~Duck()
  {
     deletQuack();
     deleteFly();

    cout << "오리 인스턴스 소멸" << endl;
  }

 // 나는 행위 추상함수 호출(구체적 행위는 숨겨짐)
 void fly()
 {
    flyBehavior->fly();
 }

 // 울음 행위 추상 함수 호출
 void  quack()
 {
    quackBehavior->quack();
 }


 // setter 함수
 // 구체적 행위를 여기서 설정 
 // 오리 울음을 바꾸는 setter함수
 void setQuackBehavior(QuackBehavior *qb)
 {
    deletQuack();  // 기 설정된 behavior가 있으면 해제
    quackBehavior = qb;
 }

 // 오리의 나는 모습을 바꾸는 setter 함수
 void setFlyBehavior(FlyBehavior *fb)
 {
    deleteFly(); // 기 설정된 behavior가 있으면 해제
    flyBehavior = fb;
 }
};



// client class를 상속 받아 행위를
// 가지는 주체를 설정한다. 
// 청둥 오리 클래스
class MallardDuck: public Duck
{
public:
 MallardDuck()
 {
    cout << "청둥오리 인스턴스 생성" << endl;
    setQuackBehavior(new Quack_Normal); //청둥오리는 Normal 하게 운다
    setFlyBehavior(new Fly_Normal); // 청둥오리는 Normal 하게 난다
 }
};

// 장난감 오리 클래스
class ToyDuck: public Duck
{
public:
 ToyDuck()
 {
    cout << "장난감 오리 인스턴스 생성" << endl;
    setQuackBehavior(new Quack_Beep); //장난감오리는 Beep로 운다
    setFlyBehavior(new Fly_None); // 장난감오리는 못 난다
 }
};


void main()
{
 // 청둥오리 인스턴스 생성
 Duck* k= new MallardDuck;
 k->quack();
 k->fly();
 delete k;

 cout <<endl<<endl;

 // 장난감 오리 인스턴스 생성
 Duck* toy = new ToyDuck;
 toy->quack();
 toy->fly();
 delete toy;

}

(예2) Pattern Matching 

(1) 행위 정의(class match, 추상클래스): SelectFeat() 가상함수를 가짐

-픽셀 밝기 사용 child: class pixelMatch: public match, SelectFeat()에서 밝기사용 매칭의 구현

-에지를 사용 child: class edgeMatch: public match, SelectFeat()에서 에지사용 매칭 구현

// Interface class
class Match
{
public:
virtual void SelectFeat()=0;
};

class pixelMatch: public Match
{
public:
void SelectFeat()
{
cout << "Matching pixels ...\n" << endl;
}
};

class edgeMatch: public Match
{
public:
void SelectFeat()
{
cout << "Matching edges ...\n" << endl;
}
};

(2) class Match를 사용하는 client class

-추상 class patFind는 멤버변수로 Match 객체를 가짐

-find()함수를 가짐: 설정된 방법으로 matching 실행

-setter함수를 통해 match 실행에서 특징 타입(밝기 또는 에지) 설정

-행위를 사용하는 주체가 되는 클래스는 patFind 클래스를 상속한다.

 class edgeFind: public patFind

 class pixelFind: public patFind

// Client class
class patFind
{
public:
patFind() { useFeat_=0; }
~patFind() { deleteFind(); }

void find()
{
cout << "Matching started..." << endl;
useFeat_->SelectFeat();
}
void SetFeat(Match *ft)
{
deleteFind();
useFeat_ = ft;
}
void deleteFind()
{
if(useFeat_)
{ 
cout << "기 설정된 특징이 있음. 제거.." << endl;
delete useFeat_;
}
}

private:
Match *useFeat_;
};


// 행위를 가지거나 사용하는 주체가 되는 클래스
class edgeFind: public patFind
{
public:
edgeFind()
{
cout << "Edge matching 인스턴스 생성" << endl;
SetFeat(new edgeMatch);
}
};

class pixelFind: public patFind
{
public:
pixelFind()
{
cout << "Pixel matching 인스턴스 생성" << endl;
SetFeat(new pixelMatch);
}
};

(3) main에서 사용

// Pattern matching 인스턴스 생성(pixel 사용 시)
patFind* find = new pixelFind();
find->find();
delete find;


// Pattern matching 인스턴스 생성(edge 사용 시)
patFind* find = new edgeFind();
find->find();
delete find;

References
[1] Head First Design Pattern, Chapter 1.
[2] 장문석, Escort GoF의 디자인 패턴, 24장
[3] D:\다운로드_2013\패턴_테스트\strg_patt