script5

Contents

produce stereo images

clear
close all

pos1 = [1 17 3];
target1 = [4 0 3];
pos2 = [5 21 2];
target2 = [2 0 3];

cam1 = camobj(pos1,target1);
cam2 = camobj(pos2,target2);
cam = cam2;


%          1      2      3      4      5      6      7      8
vcube = [ 0 0 0; 1 0 0; 1 1 0; 0 1 0; 0 0 1; 1 0 1; 1 1 1; 0 1 1]';
idx = [1 2 3 4 1 5 6 7 8 5 6 2 3 7 8 4];
vert(1,:) = vcube(1,:)*6;  % hallway width 6 ft.
vert(2,:) = vcube(2,:)*5; % length 90 ft.
vert(3,:) = vcube(3,:)*8;  % height 8 ft.


x1 = vert(1,idx);
y1 = vert(2,idx);
z1 = vert(3,idx);
plot3(x1,y1,z1,'k-');
hold on
ndx = [1 2 6 5 1];
x = vert(1,ndx);
y = vert(2,ndx);
z = vert(3,ndx);
plot3(x,y,z,'r-','LineWidth',2);
hold off

campos(cam.pos);
camtarget(cam.target);
camup([0 0 1]);
camproj('perspective');
camva(40);
set(gcf,'renderer','zbuffer')
axis equal
axis off
NPIX = 640;
set(gcf,'Position',[100 100 NPIX NPIX]);
set(gca,'Position',[0 0 1 1]);


f = getframe(gcf);
rgb = f.cdata;

verify calculate of image points from world points

pts = [x' y' z'];
cpts = pts*cam.R + cam.T;
xn = h2e_row(cpts);
ipts = h2e_row(cpts*cam.KK);
imshow(rgb);
hold on
plot(ipts(:,1),ipts(:,2),'bx','MarkerSize',11,'LineWidth',2);
hold off
axis ij

verify skew matrix

t_v1 = (cam2.pos - cam1.pos)*cam1.R
t = cam2.pos*cam1.R+cam1.T

skew = [ 0 -t(3) t(2); t(3) 0 -t(1); -t(2) t(1) 0];
skew*[xn(1,:) 1]'
% check with cross product
cross(t,[xn(1,:) 1])

t_v1 =
   -4.6343    1.0000   -3.2440
t =
   -4.6343    1.0000   -3.2440
ans =
    1.4567
    4.3338
   -0.7451
ans =
    1.4567    4.3338   -0.7451

calculate essential matrix

pts = [x1' y1' z1'];
N = length(pts);
cpts1 = pts*cam1.R + cam1.T;
c1n = h2e_row(cpts1);
c1n = [c1n ones(N,1)];
k1 = c1n*cam1.R';

cpts2 = pts*cam2.R + cam2.T;
c2n = [h2e_row(cpts2) ones(N,1)];
k2 = c1n*cam2.R';

Tw2 = -cam2.T*cam2.R';
Tw1 = -cam1.T*cam1.R';
T = Tw2 - Tw1;
T = T/norm(T);
Sk = [0 -T(3) T(2); T(3) 0 -T(1); -T(2) T(1) 0];
E = cam2.R'*Sk*cam1.R

for k=1:N
    test = c2n(k,:)*E*c1n(k,:)';
    disp(test);
end

E =
    0.0542    0.7878    0.1654
   -0.5719   -0.0278    0.8084
   -0.1387   -0.5902    0.0161
  -1.3878e-17
   1.0408e-17
     0
  -3.4694e-17
  -1.3878e-17
   2.1077e-16
   2.1467e-16
   3.9552e-16
   2.4807e-16
   2.1077e-16
   2.1467e-16
   1.0408e-17
     0
   3.9552e-16
   2.4807e-16
  -3.4694e-17

alternative construction of Essential matrix

Tw2 = -cam2.T*cam2.R';
Tw1 = -cam1.T*cam1.R';
T = (Tw1 - Tw2)*cam2.R;
T = T/norm(T);
Sk = [0 -T(3) T(2); T(3) 0 -T(1); -T(2) T(1) 0];
E = Sk*(cam2.R'*cam1.R)

for k=1:N
    test = c2n(k,:)*E*c1n(k,:)';
    disp(test);
end

E =
   -0.0542   -0.7878   -0.1654
    0.5719    0.0278   -0.8084
    0.1387    0.5902   -0.0161
   2.7756e-17
  -2.4286e-17
   2.7756e-17
   3.4694e-17
   2.7756e-17
  -2.1077e-16
  -2.1467e-16
  -3.4001e-16
  -2.4807e-16
  -2.1077e-16
  -2.1467e-16
  -2.4286e-17
   2.7756e-17
  -3.4001e-16
  -2.4807e-16
   3.4694e-17

[U, S, V] = svd(E);

u3 = U(:,3)';
norm(u3)
u3
T
% this worked because we made T a unit vector

W = [0 -1 0; 1 0 0; 0 0 1];

G1 = U*W*V'
% G2 = U*W'*V';
Rc = cam2.R'*cam1.R
max(max(abs(G1-Rc)))

ans =
    1.0000
u3 =
    0.5908   -0.1368    0.7951
T =
    0.5908   -0.1368    0.7951
G1 =
    0.9503   -0.0000   -0.3113
    0.0147    0.9989    0.0447
    0.3110   -0.0471    0.9493
Rc =
    0.9503         0   -0.3113
    0.0147    0.9989    0.0447
    0.3110   -0.0471    0.9493
ans =
   4.5103e-16

calculate fundamental matrix

K = cam1.KK;
impts1 = [h2e_row(c1n*K) ones(N,1)];
impts2 = [h2e_row(c2n*K) ones(N,1)];

% verify impts calculation
% imshow(rgb);
% hold on
% plot(impts2(:,1),impts2(:,2),'bx');
% hold off
Fc = inv(K)*E*inv(K)';
if (Fc(3,3)~=0)
     Fc = Fc/Fc(3,3);
end
Fc
for k=1:N
    test2 = impts2(k,:)*Fc*impts1(k,:)';
    disp(test2);
end

Fc =
   -0.0000   -0.0000    0.0039
    0.0000    0.0000   -0.0285
   -0.0014    0.0241    1.0000
   1.3323e-15
   3.3307e-15
   2.4425e-15
   2.6645e-15
   1.3323e-15
  -5.2180e-15
  -6.3144e-15
  -1.0658e-14
  -7.7716e-15
  -5.2180e-15
  -6.3144e-15
   3.3307e-15
   2.4425e-15
  -1.0658e-14
  -7.7716e-15
   2.6645e-15

Use MATLAB Vision toolkit function

[F,inliersIndex,status] = estimateFundamentalMatrix(impts1(:,1:2),impts2(:,1:2));
disp(F);

for k=1:N
    test2 = impts2(k,:)*F*impts1(k,:)';
    fprintf('%14g %4d\n',test2, inliersIndex(k));
end

    0.0001    0.0000   -0.1205
   -0.0002    0.0001    0.1099
    0.0603   -0.1212    0.9775
      -12.8669    0
      -1.44638    1
     -0.666351    1
      -17.1213    0
      -12.8669    0
       6.13529    0
      0.205144    1
     -0.838148    1
       14.5909    0
       6.13529    0
      0.205144    1
      -1.44638    1
     -0.666351    1
     -0.838148    1
       14.5909    0
      -17.1213    0

Published with MATLAB® R2019b