hallway4

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

up = [0 0 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

[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
vp = intersect(imgpt(1:2,:),imgpt(3:4,:),'kx')

vpx = T(1,2)/T(3,2)
vpy = T(2,2)/T(3,2)

vp =

    0.0000    0.0667


vpx =

     0


vpy =

    0.0667

% test back-projection (1)
% pn = [a b c d]
%  k = [imgpt 1]
%  p = pos + q k
pn = [0 0 1 0];
pos = [cam.pos 1];
for m=1:4
    k = invpt(m,:)-cam.pos;
    q = -(pn*pos')/(pn(1:3)*k');
    pt = cam.pos + q*k;
    pt
end

pt =

  1.0e-014 *

   -0.2665    0.5329         0


pt =

    0.0000  100.0000         0


pt =

   10.0000    0.0000         0


pt =

   10.0000  100.0000         0

% testr back-projection math (2)
Tinv = [cam.Tinv; 0 0 0 1];
pw = [0 0 1 0];
pn = pw*Tinv

for m=1:4
    k = [imgpt(m,:) 1];
    q = -pn(4)/(pn(1:3)*k');
    pt = q*k;
    cam.Tinv*[pt 1]'
end

pn =

         0    0.9978   -0.0665    2.0000


ans =

  1.0e-014 *

         0
   -0.1776
    0.0222


ans =

         0
  100.0000
         0


ans =

   10.0000
   -0.0000
    0.0000


ans =

   10.0000
  100.0000
         0

% test back-projection math (3)
d = -pn(4);
M = [d 0 0 0; 0 d 0 0; 0 0 d 0; pn(1:3) 0];
Tx = Tinv*M;
for m=1:4
    k = [imgpt(m,:) 1 1];
    pth = Tx*k';
    pt(1:3) = pth(1:3)./pth(4);
    pt
end
Tx

pt =

  1.0e-015 *

   -0.5613         0         0


pt =

    0.0000  100.0000         0


pt =

    10     0     0


pt =

   10.0000  100.0000         0


Tx =

   -2.0000    4.9889   -0.3326         0
         0  -10.1109   -1.3304         0
         0         0         0         0
         0    0.9978   -0.0665         0

% test back projection math (4)
N = 4;
k = [imgpt ones(N,2)];
pth = k*Tx';
pt = pth(:,1:3)./repmat(pth(:,4),[1 3])

pt =

   -0.0000         0         0
    0.0000  100.0000         0
   10.0000         0         0
   10.0000  100.0000         0

% use standard MATLAB cp2tform function
base_points = pts(:,1:2);
Tf = cp2tform(imgpt,base_points,'Projective');
Tf.tdata.T

ans =

   10.1109         0         0
  -25.2212   51.1150   -5.0442
    1.6814    6.7257    0.3363

% MATLAB tform transformation
out = [imgpt ones(4,1)]*Tf.tdata.T;
out = out(:,1:2)./[out(:,3) out(:,3)]

% check normalization
Tn = Tx';
scl = Tf.tdata.T(3,3)/Tn(3,4)
Tm = Tn*scl;
Tm = Tm(1:3,[1 2 4])
test = max(max(abs(Tm-Tf.tdata.T)))

out =

   -0.0000   -0.0000
   -0.0000  100.0000
   10.0000   -0.0000
   10.0000  100.0000


scl =

   -5.0554


Tm =

   10.1109         0         0
  -25.2212   51.1150   -5.0442
    1.6814    6.7257    0.3363


test =

  1.4211e-014

% plot using MATLAB camera
close all
plot3(hpts(1:2,1),hpts(1:2,2),hpts(1:2,3));
hold on
plot3(hpts(3:4,1),hpts(3:4,2),hpts(3:4,3));
camproj('Perspective');
campos(cam.pos);
camtarget(cam.target);
camva(25);
daspect([1 1 1]);
%set(gca,'ZLim',[0 2]);
axis equal
grid
hold off
Tx = view
XLim = get(gca,'XLim')
YLim = get(gca,'YLim')
ZLim = get(gca,'ZLim')

Tx =

    0.1000         0         0   -0.0500
         0    0.0665    0.0998   -0.0632
         0    0.9978   -0.0067    0.1044
         0         0         0    1.0000


XLim =

     0    10


YLim =

     0   100


ZLim =

    -5     5