视觉(15)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 00 -1 0]';
trans_axis = [1 0 00 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;

Powered by Jekyll and Theme by solid

本站总访问量