hallway5

clear, close all
w = 10;
d = 100;
pts = [ 0 0 0; 0 d 0; w 0 0; w d 0];
N = 4;
hpts = [pts  ones(N,1)];

cam.pos = [5 -10 2];
cam.target = [5 20 0];
cam = cam_obj(cam);

T = cam.T

T =

    1.0000         0         0   -5.0000
         0    0.0665    0.9978   -1.3304
         0    0.9978   -0.0665   10.1109

% calculate image points and show vanishing point
[imgpt invpt] = cam_plot(cam,pts);
plot(imgpt(1:2,1),imgpt(1:2,2),'g');
hold on;
plot(imgpt(3:4,1),imgpt(3:4,2),'r');
plot(0,0,'ko','MarkerSize',5);
hold off
axis equal
% calculate vanishing point
vpy = intersect(imgpt(1:2,:),imgpt(3:4,:),'kx')
vpx = [Inf NaN];
p = [vpx; vpy];

vpy =

    0.0000    0.0667

% back-projection, using camera matrix
pn = [0 0 1 0];
Tinv = [cam.Tinv; 0 0 0 1];
Tinv(1:2,4)=0.0;
Tinv(3,4)=2;
pw = [0 0 1 0];
pn = pw*Tinv
d = -pn(4);
M = [d 0 0 0; 0 d 0 0; 0 0 d 0; pn(1:3) 0];
Tx = Tinv*M;
N = 4;
k = [imgpt ones(N,2)];
pth = k*Tx';
pto = pth(:,1:3)./repmat(pth(:,4),[1 3])

draw5(pto)

pn =

         0    0.9978   -0.0665    2.0000


pto =

   -5.0000   10.0000         0
   -5.0000  110.0000         0
    5.0000   10.0000         0
    5.0000  110.0000         0

% search for camera properties, using vanishing point data
%v = [0 0.1 0 0.5];
v = [0 -5 0 -2];
[vfit fmin exitflag options] = fminsearch(@fval2,v,[],p)

 
Exiting: Maximum number of function evaluations has been exceeded
         - increase MaxFunEvals option.
         Current function value: 0.000000 


vfit =

    0.0029   -4.6139    0.0016    0.8261


fmin =

  1.8262e-009


exitflag =

     0


options = 

    iterations: 482
     funcCount: 801
     algorithm: 'Nelder-Mead simplex direct search'
       message: [1x149 char]

pto = back_projection(vfit,imgpt);
draw5(pto);

%v = [0  -.1 -0.02 2];
v = [0 -5 0 -2];
[vfit fmin exitflag options] = fminsearch(@fval3,v,[],imgpt)

vfit =

    0.0005   -3.8141    0.0002   -2.0000


fmin =

  2.4299e-005


exitflag =

     1


options = 

    iterations: 65
     funcCount: 119
     algorithm: 'Nelder-Mead simplex direct search'
       message: [1x196 char]

[pto Tn pn] = back_projection(vfit,imgpt);
draw5(pto);

% show errors
[y q] = fval3(vfit,imgpt)

y =

  2.4299e-005


q =

    0.0001   -0.0002    0.0005    0.0049   -0.0001

% move the camera
cam2.pos = [7 -10 2];
cam2.target = [2 20 0];
cam2 = cam_obj(cam2);

% calculate image points and show vanishing point
imgpt2 = cam_plot(cam2,pts);
plot(imgpt2(1:2,1),imgpt2(1:2,2),'g');
hold on;
plot(imgpt2(3:4,1),imgpt2(3:4,2),'r');
plot(0,0,'ko','MarkerSize',5);
hold off
axis equal
% calculate vanishing point
vpy = intersect(imgpt2(1:2,:),imgpt2(3:4,:),'kx')

vpy =

    0.1670    0.0658

% draw hallway based on original camera calibration
pto = back_projection(vfit,imgpt2);
draw5(pto);

ptc = back_projection(vfit,[0 0]);
hold on
plot(0,0,'k+');
plot([0 ptc(:,1)],[0 ptc(:,2)],'k');
hold off