视觉SFM一例

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

原文地址：http://www.cnblogs.com/cutepig/archive/2007/08/14/855285.html