这里采用的是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;