%Homework 5 - Computer Vision - Benjamin Berger
%CISC 689 - University of Delaware - 05/04/03
clf, figure(1);

%===============================================================%
% Part 2 - Factorization for SFM 
%===============================================================%

I = (im2double(imread('hotel.seq0.png')));
[ri,ci,di] = size(I);

imshow(I); hold on; zoom on;

%read in feature locations detected by the KLT open source tracker: 
points = dlmread(['features.dlm'],','); 
[rp,cp] = size(points);

%create a plot showing the feature locations in each frame,
%plotted over the first frame

for j=1:rp %for all 150 features
  plot(points(j,2),points(j,3),'g+'); 
end 

for i=1:(cp-1)/3 %for all 100 frames
  for j=1:rp %for all 150 features
    %if feature lost, plot red cross:  
    if (points(j,i*3-1)<0) & (points(j,(i-1)*3-1)>-1)
      plot(points(j,(i-1)*3-1),points(j,(i-1)*3),'rx'); 
      %feature_x_lost_in_frame_y=[j i]
    else
      plot(points(j,i*3-1),points(j,i*3),'y.'); 
    end 
  end
end

%2nd:
%take only the rows with features tracked through whole sequence:
X_im=points(all(points+1,2),:)';

%take only x and y coloumns:
X_im(1:3:cp,:)=[];
[rx,cx] = size(X_im);

%call factorize:
[M, t, X]=factorize(X_im);

figure(2)
plot3(X(2,:),X(1,:),X(3,:),'b.');
axis([-200 200 -200 200 -200 600])
hold on

%arbitrary, reasonable depth:
d=500;

%plot camera locations into 3D space, use inverse camera matrices 
%to transform 2D camera location (middle of image with amiguity depth) in 3D world coordinates
for i=1:rx/2
    K=[M(2*i-1:2*i,:);[0 0 1]];
    Xcam2(:,i)=inv(K)*[0 0 d]'; 
end

plot3(Xcam2(2,:),Xcam2(1,:),Xcam2(3,:),'rx');
print -djpeg90 -r100 3D_ViewC8