视觉（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 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;

Published on Aug 14, 2007 in categories