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.