script4

clear
close all
load cdata
n = 1;
filename = files{n};
p = pts(:,:,n);
rgb = imread(filename);

clear ipts
wpts(1,:)
ipts(1,:) = p(1,:);
wpts(8,:)
ipts(2,:) = p(8,:);
wpts(57,:)
ipts(3,:) = p(57,:);
wpts(64,:)
ipts(4,:) = p(64,:);

figure(1)
imshow(rgb);

hold on
plot(ipts(:,1),ipts(:,2),'ro','MarkerSize',11);
hold off
ans =

     0     0


ans =

     0    21


ans =

    21     0


ans =

    21    21

Warning: Image is too big to fit on screen; displaying at 25% 
k(1,:) = ipts(3,:) - ipts(1,:);
k(2,:) = ipts(4,:) - ipts(2,:);
k(3,:) = ipts(2,:) - ipts(1,:);
k(4,:) = ipts(4,:) - ipts(3,:);
for np=1:4
    k(np,:) = k(np,:)/norm(k(np,:));
end

hold on
N = 500;
plot(ipts(1,1)+[0 N*k(1,1)],ipts(1,2)+[0 N*k(1,2)],'r','LineWidth',3);
plot(ipts(2,1)+[0 N*k(2,1)],ipts(2,2)+[0 N*k(2,2)],'g','LineWidth',3);
plot(ipts(1,1)+[0 N*k(3,1)],ipts(1,2)+[0 N*k(3,2)],'b','LineWidth',3);
plot(ipts(3,1)+[0 N*k(4,1)],ipts(3,2)+[0 N*k(4,2)],'y','LineWidth',3);
hold off
sr(1) = det(k(1:2,:));
sr(2) = det(k(3:4,:));
sr = asin(sr)*180/pi
sr(1)^2+sr(2)^2
sr =

      -6.0054       2.9162


ans =

       44.569

if n==1
    ang = [14.255 -30.31 0]; % image 1
else
    ang = [20.104 60.535 -4.9 ]; % image 2
end
KK = [6500 0 0; 0 6500 0; 2000 1000 1];
R = rotx(ang(1),'deg')*roty(ang(2),'deg')*rotz(ang(3),'deg');
tf = projective2d(inv(KK)*R*KK);

[x, y] = transformPointsForward(tf,ipts(:,1),ipts(:,2));
figure(2)
plot(x,y,'ko');
axis ij
axis equal

tpts = [x y]

k(1,:) = tpts(3,:) - tpts(1,:);
k(2,:) = tpts(4,:) - tpts(2,:);
k(3,:) = tpts(2,:) - tpts(1,:);
k(4,:) = tpts(4,:) - tpts(3,:);
for kp=1:4
    k(kp,:) = k(kp,:)/norm(k(kp,:));
end
s(1) = det(k(1:2,:));
s(2) = det(k(3:4,:));
s = asin(s)*180/pi
s(1)^2+s(2)^2

fprintf('divergence angles (deg) %.4g %.4g\n',s);
k
tpts =

       4576.5       2270.6
       4639.2       3807.2
         6134       2259.9
       6196.7       3796.6


s =

   4.5052e-07   0.00044551


ans =

   1.9848e-07

divergence angles (deg) 4.505e-07 0.0004455

k =

      0.99998   -0.0068074
      0.99998   -0.0068074
     0.040812      0.99917
     0.040804      0.99917

if n==1
    myref = imref2d([640 640],[4000 8000],[2000 6000]); % image 1
else
    myref = imref2d([640 640],[-7000 -3000],[3000 7000]); % image 2
end
[img, ref] = imwarp(rgb,tf,'OutputView',myref,'FillValues',[255 255 200]);
figure(3);
imshow(img);
ref

hold on
[xInt,yInt] = worldToIntrinsic(ref,tpts(:,1),tpts(:,2));
plot(xInt,yInt,'ro','MarkerSize',11,'MarkerFaceColor','y');
hold off
ref = 

  imref2d with properties:

           XWorldLimits: [4000 8000]
           YWorldLimits: [2000 6000]
              ImageSize: [640 640]
    PixelExtentInWorldX: 6.25
    PixelExtentInWorldY: 6.25
    ImageExtentInWorldX: 4000
    ImageExtentInWorldY: 4000
       XIntrinsicLimits: [0.5 640.5]
       YIntrinsicLimits: [0.5 640.5]

N = 31;
[xm ym] = meshgrid(linspace(10,20,N),linspace(-35,-25,N));

zm = zeros([size(xm) 2]);

for nx=1:N
    for ny=1:N
        theta = xm(nx,ny);
        phi = ym(nx,ny);
        R = rotx(theta,'deg')*roty(phi,'deg')*rotz(ang(3),'deg');
        tf = projective2d(inv(KK)*R*KK);

        [x, y] = transformPointsForward(tf,ipts(:,1),ipts(:,2));

        tpts = [x y];

        k(1,:) = tpts(3,:) - tpts(1,:);
        k(2,:) = tpts(4,:) - tpts(2,:);
        k(3,:) = tpts(2,:) - tpts(1,:);
        k(4,:) = tpts(4,:) - tpts(3,:);
        for kp=1:4
            k(kp,:) = k(kp,:)/norm(k(kp,:));
        end
        s(1) = det(k(1:2,:));
        s(2) = det(k(3:4,:));
        s = asin(s)*180/pi;
        zm(nx,ny,:) = s;
    end
end

vm = zm(:,:,1).^2+zm(:,:,2).^2;
figure(4)
mesh(xm,ym,vm)
[vm1, idx] = min(vm);
[vm2, jdx] = min(vm1);
vm2
n1=idx(jdx);
n2=jdx;
fprintf('minimum %g at theta %g phi %g\n',vm(n1,n2),xm(n1,n2),ym(n1,n2));
vm2 =

    0.0002843

minimum 0.000284303 at theta 14.3333 phi -30.3333
if n==1
    ang = [15 -30 0];
else
    ang = [20.104 60.535 0];
end

theta = ang(1);
phi = ang(2);
R = rotx(theta,'deg')*roty(phi,'deg')*rotz(ang(3),'deg');
tf = projective2d(inv(KK)*R*KK);

[x, y] = transformPointsForward(tf,ipts(:,1),ipts(:,2));

tpts = [x y];

k(1,:) = tpts(3,:) - tpts(1,:);
k(2,:) = tpts(4,:) - tpts(2,:);
k(3,:) = tpts(2,:) - tpts(1,:);
k(4,:) = tpts(4,:) - tpts(3,:);
for kp=1:4
    k(kp,:) = k(kp,:)/norm(k(kp,:));
end
s(1) = det(k(1:2,:));
s(2) = det(k(3:4,:));
s = asin(s)*180/pi;
b = s';


theta = ang(1) + 0.1;
phi = ang(2);
R = rotx(theta,'deg')*roty(phi,'deg')*rotz(ang(3),'deg');
tf = projective2d(inv(KK)*R*KK);

[x, y] = transformPointsForward(tf,ipts(:,1),ipts(:,2));

tpts = [x y];

k(1,:) = tpts(3,:) - tpts(1,:);
k(2,:) = tpts(4,:) - tpts(2,:);
k(3,:) = tpts(2,:) - tpts(1,:);
k(4,:) = tpts(4,:) - tpts(3,:);
for kp=1:4
    k(kp,:) = k(kp,:)/norm(k(kp,:));
end
s(1) = det(k(1:2,:));
s(2) = det(k(3:4,:));
s = asin(s)*180/pi;
A(:,1) = s'-b;


theta = ang(1);
phi = ang(2)+0.1;
R = rotx(theta,'deg')*roty(phi,'deg')*rotz(ang(3),'deg');
tf = projective2d(inv(KK)*R*KK);

[x, y] = transformPointsForward(tf,ipts(:,1),ipts(:,2));

tpts = [x y];

k(1,:) = tpts(3,:) - tpts(1,:);
k(2,:) = tpts(4,:) - tpts(2,:);
k(3,:) = tpts(2,:) - tpts(1,:);
k(4,:) = tpts(4,:) - tpts(3,:);
for kp=1:4
    k(kp,:) = k(kp,:)/norm(k(kp,:));
end
s(1) = det(k(1:2,:));
s(2) = det(k(3:4,:));
s = asin(s)*180/pi;
A(:,2) = s'-b;

sol = ang(1:2)'-A\b*0.1
sol =

       14.255
       -30.31