這里采用的是Yi Ma , Stefano Soatto. An Invitation to 3-D Vision , From Images to Geometric Models 的算法
%// Algorithm 8.1. also 11.7
%// Rank based factorization algorithm for multiview reconstruction
%// using point features
%// as described in Chapter 8, "An introduction to 3-D Vision"
%// by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS)
%// Code distributed free for non-commercial use
%// Copyright (c) MASKS, 2003
%// Generates multiple synthetic views of a house and computes the
%// motion and structure, calibrated case, point features only
%// Jana Kosecka, George Mason University, 2002
%// ======================================================================
close all; clear;
FRAMES = 3;
PLOTS = 3;
%// transformation is expressed wrt to the camera frame
Zinit = 5;
%// cube in the object frame
XW = [0 1 1 0 0 1 1 0 0.2 0.8 0.2 0.8 ;
0 0 1 1 0 0 1 1 1.5 1.5 1.5 1.5;
1 1 1 1 0 0 0 0 0.8 0.8 0.2 0.2 ;
1 1 1 1 1 1 1 1 1 1 1 1];
NPOINTS = 12;
XC = zeros(4,NPOINTS,FRAMES);
%// initial displacement攝像機的初始位置
Rinit = rot_matrix([1 1 1],0);
Tinit = [ Rinit(1,:) -0.5 ;
Rinit(2,:) -0.5 ;
Rinit(3,:) Zinit;
0 0 0 1];
%// first camera coodinates
XC(:,:,1) = Tinit*XW;
%//畫出三維的結構 original motion and 3D structure
figure; hold on;
plot3_struct(XC(1,:,1),XC(2,:,1),XC(3,:,1));
plot3(XC(1,:,1),XC(2,:,1),XC(3,:,1),'*');
draw_frame_scaled([diag([1,1,1]), zeros(3,1)],0.5);
title('original motion and 3D structure');
view(220,20);
grid on; axis equal;
%// axis off;
pause;
%// image coordinates 計算第一幀時的圖像坐標
xim(:,:,1) = project(XC(:,:,1));
Zmax = max(XC(3,:,1));
Zmin = min(XC(3,:,1));
rinc = pi/30;
rot_axis = [1 0 0; 0 -1 0]';
trans_axis = [1 0 0; 0 1 0]';
ratio = 1;
rinc = 10; %// rotation increment 20 degrees
Zmid = (Zmax+Zmin)/2;
tinc = 0.5*ratio*Zmid*rinc*pi/180;
ploting = 1;
for i=2:FRAMES %//計算第i幀的圖像坐標xim
theta = (i-1)*rinc*pi/180;
r_axis = rot_axis(:,i-1)/norm(rot_axis(:,i-1));
t_axis = trans_axis(:,i-1)/norm(trans_axis(:,i-1));
trans = (i-1)*tinc*t_axis;
R = rot_matrix(r_axis,theta);
%// translation represents origin of the camera frame
%// in the world frame
T(:,:,i) = ([ R trans;
0 0 0 1]);
%// all transformation with respect to the object frame
XC(:,:,i) = T(:,:,i)*XC(:,:,1); %// XW;
draw_frame_scaled(T(1:3,:,i),0.5);
xim(:,:,i) = [XC(1,:,i)./XC(3,:,i); XC(2,:,i)./XC(3,:,i);
ones(1,NPOINTS)];
end;
for j = 2:FRAMES
T_ini(:,j) = T(1:3,4,j);
end;
%// noise can be added here
for i=1:FRAMES
xim_noisy(:,:,i) = xim(:,:,i);
end
%// pause 以下為SFM算法
%//---------------------------------------------------------------------
%// compute initial \alpha's for each point using first two frames only 1）首先用八點算法計算初始的R0,T0(我感覺T0~即1，0幀之間的相對移動~和實際的應該相差常數倍，因此會導致恢復的結構和實際相差常數倍)，然后估計lambda。。。
[T0, R0] = essentialDiscrete(xim_noisy(:,:,1),xim_noisy(:,:,2));
for i = 1:NPOINTS
alpha(:,i) = -(skew(xim_noisy(:,i,2))*T0)'*
(skew(xim_noisy(:,i,2))*R0*xim_noisy(:,i,1))
/(norm(skew(xim_noisy(:,i,2))*T0))^2;
lambda(:,i) = 1/alpha(:,i);
end
scale = norm(alpha(:,1)); %// set the global scale
alpha = alpha/scale; %// normalize everything
scale = norm(lambda(:,1)); %// set the global scale
lambda = lambda/scale; %// normalize everything
%//---------------------------------------------------------------------
%// Compute initial motion estimates for all frames
%// Here do 3 iterations - in real setting look at the change of scales
iter = 1;
while (iter < 5);
for j = 2:FRAMES
P = []; %// setup matrix P
for i = 1:NPOINTS
a = [kron(skew(xim_noisy(:,i,j)),xim(:,i,1)')
alpha(:,i)*skew(xim_noisy(:,i,j))];
P = [P; a];
end;
%// pause
[um, sm, vm] = svd(P);
Ti = vm(10:12,12);
Ri = transpose(reshape(vm(1:9,12)',3,3));
[uu,ss,vv] = svd(Ri);
Rhat(:,:,j) = sign(det(uu*vv'))*uu*vv';
Ti = sign(det(uu*vv'))*Ti/((det(ss))^(1/3));
That(:,j) = Ti;
True = T(1:3,4,j);
end
%// recompute alpha's based on all views
lambda_prev = lambda;
for i = 1:NPOINTS
M = []; %// setup matrix M
for j=2:FRAMES %// set up Hl matrix for all m views
a = [ skew(xim(:,i,j))*That(:,j)
skew(xim(:,i,j))*Rhat(:,:,j)*xim(:,i,1)];
M = [M; a];
end;
a1 = -M(:,1)'*M(:,2)/norm(M(:,1))^2;
lambda(:,i) = 1/a1;
end;
scale = norm(lambda(:,1)); %// set the global scale
lambda = lambda/scale; %// normalize everything
iter = iter + 1
end %// end while iter
%// final structure with respect to the first frame
XF = [lambda.*xim(1,:,1);
lambda.*xim(2,:,1);
lambda.*xim(3,:,1)];
figure; hold on;
plot3(XF(1,:,1),XF(2,:,1),XF(3,:,1),'r*');
plot3_struct(XF(1,:,1), XF(2,:,1), XF(3,:,1));
title('recovered structure');
view(220,20);
grid on; axis equal;
%// axis off;
pause;