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];
else
ang = [20.104 60.535 -4.9 ];
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]);
else
myref = imref2d([640 640],[-7000 -3000],[3000 7000]);
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