This is the Yi Ma, Stefano soatto. An invitation to-Z Vision, the algorithm from the 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, "a Introduction to-Z Vision"
%//by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (Masks)
%//Code distributed free to 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 The initial position of the camera
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;
%//draws a three-dimensional structure 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 calculating the coordinates of the first frame
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 degrees
Zmid = (zmax+zmin)/2;
Tinc = 0.5*ratio*zmid*rinc*pi/180;
ploting = 1;
For I=2:frames%//calculates the image coordinates of frame 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 is added here
For I=1:frames
Xim_noisy (:,:, i) = Xim (:,:, i);
End
%//pause below is the SFM algorithm
%//---------------------------------------------------------------------
%//Compute initial \alpha ' for each point using first twice frames only 1) The initial r0,t0 is calculated using the eight-points algorithm (I feel that the t0~ is 1, the relative movement between 0 frames ~ and the actual should be a constant number of times , thus causing the structure of the recovery to be often several times different from the actual, and then estimating the 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 does 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
Visual SFM algorithm
