视觉SFM的一个例子

这里采用的是Yi Ma , Stefano Soatto. An Invitation to 3-D Vision , From Images to Geometric Models 的算法%//Algorithm8.1.also11.7%//Rank based factorization algorithm for multiview reconstruction%//using point features%//as described in Chapter8,"An introduction to3-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%//======================================================================close all;clear;FRAMES=3;PLOTS=3;%//transformation is expressed wrt to the camera frameZinit=5;%//cube in the object frameXW=[011001100.20.80.20.8;001100111.51.51.51.5;111100000.80.80.20.2;111111111111];NPOINTS=12;XC=zeros(4,NPOINTS,FRAMES);%//initial displacement摄像机的初始位置Rinit=rot_matrix([111],0);Tinit=[Rinit(1,:)-0.5;Rinit(2,:)-0.5;Rinit(3,:)Zinit;%//first camera coodinatesXC(:,:,1)=Tinit*XW;%//画出三维的结构original motion and3D structurefigure;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 and3D 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=[100;0-10]';trans_axis=[100;010]';ratio=1;Zmid=(Zmax+Zmin)/2;tinc=0.5*ratio*Zmid*rinc*pi/180;ploting=1;for i=2:FRAMES%//计算第i帧的图像坐标ximtheta=(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 frameT(:,:,i)=([R trans;0001]);%//all transformation with respect to the object frameXC(:,:,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:FRAMEST_ini(:,j)=T(1:3,4,j);%//noise can be added herefor i=1:FRAMESxim_noisy(:,:,i)=xim(:,:,i);end%//pause以下为SFM算法%//---------------------------------------------------------------------%//compute initial\alpha's for each point using first two frames onl y1）首先用八点算法计算初始的R0,T0(我感觉T0~即1，0帧之间的相对移动~和实际的应该相差常数倍，因此会导致恢复的结构和实际相差常数倍)，然后估计lambda。

[T0,R0]=essentialDiscrete(xim_noisy(:,:,1),xim_noisy(:,:,2));for i=1:NPOINTSalpha(:,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);endscale=norm(alpha(:,1));%//set the global scalealpha=alpha/scale;%//normalize everythingscale=norm(lambda(:,1));%//set the global scale%//---------------------------------------------------------------------%//Compute initial motion estimates for all frames%//Here do3iterations-in real setting look at the change of scalesiter=1;while(iter<5);for j=2:FRAMESP=[];%//setup matrix Pfor i=1:NPOINTSa=[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 viewslambda_prev=lambda;for i=1:NPOINTSM=[];%//setup matrix Mfor j=2:FRAMES%//set up Hl matrix for all m viewsa=[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 scalelambda=lambda/scale;%//normalize everythingiter=iter+1end%//end while iter%//final structure with respect to the first frameXF=[lambda.*xim(1,:,1);lambda.*xim(2,:,1);lambda.*xim(3,:,1)];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;。