script1

clear
close all
load cpts  % contains cpts1 and cpts2
load mpts  % contains mpts1 and mpts2
load CalibResults

figure(1);
rgb1 = imread(files{1});
show_points(rgb1,cpts1);
title([int2str(size(cpts1,1)) ' points']);

figure(2);
rgb2 = imread(files{2});
show_points(rgb2,cpts2);
title([int2str(size(cpts2,1)) ' points']);

Warning: Image is too big to fit on screen; displaying at 33% 
Warning: Image is too big to fit on screen; displaying at 33% 

worldPoints = [ 0 0; 21 0; 21 21; 0 21];

[R1, T1] = extrinsics(cpts1(1:4,:),worldPoints,cameraParams)

[R2, T2] = extrinsics(cpts2(1:4,:),worldPoints,cameraParams)

R1 =

      0.96888     -0.12796      0.21191
      0.24754      0.50279      -0.8282
  -0.00057246      0.85489      0.51882


T1 =

      -11.718      -3.6801        62.27


R2 =

      0.99939    0.0096765     0.033588
     0.024548      0.48976     -0.87151
    -0.024883       0.8718      0.48923


T2 =

      -9.8666      -5.8465       69.849

[F, inliersIndex] = estimateFundamentalMatrix(mpts1,mpts2);
format shortg
F
format
inliersIndex'

% Fundamental matrix should be rank 2
fprintf('rank(F) = %d\n',rank(F));

F =

  -1.6172e-08  -4.1798e-07  -0.00098563
   3.4252e-07  -9.3557e-08    0.0026743
   0.00064487   -0.0021594      0.99999


ans =

  Columns 1 through 13

     0     1     0     1     0     1     0     1     0     1     1     0     0

  Columns 14 through 25

     0     0     1     1     1     1     0     0     0     1     1     1

rank(F) = 2

%   mpts2 * F * mpts1' = 0;

clear dev
npts = size(mpts1,1);

for k = 1:npts
    dev(k) = [mpts2(k,1:2) 1]*F*[mpts1(k,1:2) 1]';
end

ndx = 1:npts;
[ndx' inliersIndex dev']

ans =

    1.0000         0   -0.0250
    2.0000    1.0000   -0.0015
    3.0000         0    0.0041
    4.0000    1.0000    0.0017
    5.0000         0    0.0133
    6.0000    1.0000    0.0013
    7.0000         0   -0.0260
    8.0000    1.0000   -0.0007
    9.0000         0    0.0088
   10.0000    1.0000    0.0006
   11.0000    1.0000    0.0032
   12.0000         0    0.0145
   13.0000         0    0.0079
   14.0000         0    0.0133
   15.0000         0   -0.0156
   16.0000    1.0000    0.0013
   17.0000    1.0000    0.0013
   18.0000    1.0000    0.0014
   19.0000    1.0000    0.0002
   20.0000         0    0.0287
   21.0000         0    0.0210
   22.0000         0    0.0166
   23.0000    1.0000    0.0009
   24.0000    1.0000    0.0014
   25.0000    1.0000    0.0020

% function d = computeDistance(disType, pts1h, pts2h, f)
% pfp = (pts2h' * f)';
% pfp = pfp .* pts1h;
% d = sum(pfp, 1) .^ 2;
%
% if strcmp(disType, 'sampson')
%   epl1 = f * pts1h;
%   epl2 = f' * pts2h;
%   d = d ./ (epl1(1,:).^2 + epl1(2,:).^2 + epl2(1,:).^2 + epl2(2,:).^2);
% end

k=5;
pts1h = [mpts1(k,1:2) 1]';
pts2h = [mpts2(k,1:2) 1]';
pfp = (pts2h' * F)'
pfp = pfp .* pts1h
d = sum(pfp,1).^2
fprintf('Quadratic distance %g\n',sqrt(d));

disType = 'sampson';
if strcmp(disType, 'sampson')
  epl1 = F * pts1h;
  epl2 = F' * pts2h;
  d = d ./ (epl1(1,:).^2 + epl1(2,:).^2 + epl2(1,:).^2 + epl2(2,:).^2);
  fprintf('Sampson distance %g\n',d)
end
fprintf('Our Deviation %g\n',dev(k))

pfp =

    0.0009
   -0.0036
    0.0419


pfp =

    2.7505
   -2.7791
    0.0419


d =

   1.7573e-04

Quadratic distance 0.0132562
Sampson distance 6.10084
Our Deviation 0.0132562

camera pose does not match results from extrinsics

[orientation,location] = cameraPose(F,cameraParams,mpts1,mpts2)

guess = R1*R2'

dt = T1-T2;
dt = dt/norm(dt)

orientation =

    0.9775   -0.1454    0.1530
    0.1367    0.9884    0.0656
   -0.1607   -0.0432    0.9861


location =

    0.8245    0.2578   -0.5037


guess =

    0.9742   -0.2236   -0.0320
    0.2244    0.9741    0.0270
    0.0251   -0.0335    0.9991


dt =

   -0.2287    0.2676   -0.9360

Published with MATLAB® R2016a