Colorado School of Mines
Computer Vision
Professor William Hoff
Dept of Electrical Engineering &Computer Science
Colorado School of Mines
Computer Vision
http://inside.mines.edu/~whoff/
1
Structure From Motion
Colorado School of Mines
Computer Vision
2
The Essential Matrix
• Is the matrix E, that relates the image of a point in one camera
to its image in the other camera, given a translation and
rotation
P
pT0 E p1  0
p0
• where
E = [t]x R
Z0
X0
Recall a x b = [ax]b, where
 0

a   a3
 a
 2
Colorado School of Mines
 a3
0
a1
p1
a2 

 a1 
0 
Y0
X1
Z1
Y1
• We can calculate E if we know the pose
between the two views
• Or, we can calculate E from a set of known
point correspondences
Computer Vision
3
Generate Two Synthetic Images
• Run program “createpts.m”
– Create some points on the face of cube
– Render image from two views
– Saves results: “I1.tif, I2.tif”
Colorado School of Mines
Computer Vision
4
% Create an image pair with known rotation and translation between the
% views, and corresponding image points.
clear all
close all
L = 300;
% size of image in pixels
I1 = zeros(L,L);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set up second view.
% Define rotation of camera1 with respect to camera2
ax = 0 * DEG_TO_RAD;
ay = -25 * DEG_TO_RAD;
az = 0;
Rx = [ 1
0
0;
0
cos(ax)
-sin(ax);
0
sin(ax)
cos(ax) ];
Ry = [ cos(ay)
0
sin(ay);
0
1
0;
-sin(ay)
0
cos(ay) ];
Rz = [ cos(az)
-sin(az)
0;
sin(az)
cos(az)
0;
0
0
1 ];
R_c2_c1 = Rx * Ry * Rz;
% Define f, u0, v0
f = L;
u0 = L/2;
v0 = L/2;
% Create the matrix of intrinsic camera parameters
K = [ f 0 u0;
0 f v0;
0 0
1];
% Define translation of camera2 with respect to camera1
Pc2org_c1 = [3; 0; 1];
DEG_TO_RAD = pi/180;
% Create some points on
P_M = [
0
0
0
0
0
2
1
0
2
1
0
0
0
-1 -1
1
1
1
1
1
];
NPTS = length(P_M);
the face of a cube
0
0
-1
1
0
2
-2
1
0
1
-2
1
0
0
-2
1
1
0
0
1
2
0
0
1
1
0
-1
1
2
0
-1
1
1
0
-2
1
2;
0;
-2;
1;
R_m_c2 = H_m_c2(1:3,1:3);
Pmorg_c2 = H_m_c2(1:3,4);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define pose of model with respect to camera1
ax = 120 * DEG_TO_RAD;
ay = 0 * DEG_TO_RAD;
az = 60 * DEG_TO_RAD;
Rx = [ 1
0
0;
0
cos(ax)
-sin(ax);
0
sin(ax)
cos(ax) ];
Ry = [ cos(ay)
0
sin(ay);
0
1
0;
-sin(ay)
0
cos(ay) ];
Rz = [ cos(az)
-sin(az)
0;
sin(az)
cos(az)
0;
0
0
1 ];
R_m_c1 = Rx * Ry * Rz;
Pmorg_c1 = [0; 0; 5];
% translation of model wrt camera
M = [ R_m_c1 Pmorg_c1 ];
% Figure out pose of model wrt camera 2.
H_m_c1 = [ R_m_c1 Pmorg_c1 ; 0 0 0 1];
H_c2_c1 = [ R_c2_c1 Pc2org_c1 ;
0 0 0 1];
H_c1_c2 = inv(H_c2_c1);
H_m_c2 = H_c1_c2 * H_m_c1;
% Extrinsic camera parameter matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Render image 1
p1 = M * P_M;
p1(1,:) = p1(1,:) ./ p1(3,:);
p1(2,:) = p1(2,:) ./ p1(3,:);
p1(3,:) = p1(3,:) ./ p1(3,:);
u1 = K * p1; % Convert image points from normalized to unnormalized
for i=1:length(u1)
x = round(u1(1,i));
y = round(u1(2,i));
I1(y-2:y+2, x-2:x+2) = 255;
end
figure(1), imshow(I1, []), title('View 1');
pause
Colorado School of Mines
% Extrinsic camera parameter matrix
M = [ R_m_c2 Pmorg_c2 ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Render image 2
I2 = zeros(L,L);
p2 = M * P_M;
p2(1,:) = p2(1,:) ./ p2(3,:);
p2(2,:) = p2(2,:) ./ p2(3,:);
p2(3,:) = p2(3,:) ./ p2(3,:);
% Convert image points from normalized to unnormalized
u2 = K * p2;
for i=1:length(u2)
x = round(u2(1,i));
y = round(u2(2,i));
I2(y-2:y+2, x-2:x+2) = 255;
end
figure(2), imshow(I2, []), title('View 2');
disp('Points in image 1:');
disp(u1);
disp('Points in image 2:');
disp(u2);
imwrite(I1, 'I1.tif');
imwrite(I2, 'I2.tif');
% This is the "true" essental matrix between the views
t = Pc2org_c1;
Etrue = [ 0 -t(3) t(2); t(3) 0 -t(1); -t(2) t(1) 0] * R_c2_c1;
disp('True essential matrix:');
disp(Etrue);
Computer Vision
5
Results
• True relative pose, H_c2_c1:
• Corresponding points in images:
H_c2_c1 =
0.9063
0 -0.4226 3.0000
0 1.0000
0
0
0.4226
0 0.9063 1.0000
0
0
0 1.0000
Points in image 1:
Columns 1 through 10
61.4195 102.1798 150.0000 68.3768 106.2098 150.0000 74.3208 109.6134 150.0000 176.0870
124.4290 136.1955 150.0000 167.2490 181.1490 197.2377 203.8325 219.1146 236.6025 127.4080
1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000
Columns 11 through 15
196.1538 174.0000 192.8571 172.2222 190.0000
110.0296 170.7846 150.0000 207.7350 184.6410
1.0000 1.0000 1.0000 1.0000 1.0000
Points in image 2:
Columns 1 through 10
45.5272 63.4568 86.9447 61.6620 80.2653 104.1468 75.7981 94.7507 118.6606 135.5451
126.5989 136.7293 150.0000 165.9997 180.2731 198.5963 200.5196 217.7991 239.5982 125.7710
1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000 1.0000
Columns 11 through 15
176.3357 147.5739 184.5258 157.8633 191.6139
105.4355 172.3407 150.0000 212.1766 188.5687
1.0000 1.0000 1.0000 1.0000 1.0000
• Essential matrix:
Colorado School of Mines
True essential matrix:
0 -1.0000
0
-0.3615
0 -3.1415
0 3.0000
0
Computer Vision
6
Calculating the Essential Matrix
• We have
pT0 E p1  0
• We have a set of known point correspondences p0 and p1
• We have one equation for each point correspondence
• We can solve for the unknowns in E
• Note that E is a 3x3 matrix with 9 unknowns
– It’s only known up to a scale factor
– We really only have 8 unknowns
• We need at least 8 equations – we can compute E from eight
or more point correspondences
Colorado School of Mines
Computer Vision
7
Calculating the Essential Matrix
• We have
x0
pT0 E p1  0
y0
• Write out as equation
x0
y0
 E11

1 E21
E
 31
E22
E32
E11 x0 x1  E12 x0 y1  E13 x0 
 E11 x1  E12 y1  E13 


1 E21 x1  E22 y1  E13   0
E x  E y  E 
32 1
33 
 31 1
E21 y0 x1  E22 y0 y1  E13 y0 
E31 x1  E32 y1  E33  0
• Write as A x = 0, where x = (E11, E12, E13, … , E33)
x0 x1
x0 y1
x0
y0 x1
E13  x1 
 
E23  y1   0
E33  1 
E12
y0 y1
y0
x1
y1
 E11 


 E12 
1 E13   0


  
E 
 33 
Actually, A will have one row for each point correspondence (x0,y0) – (x1,y1)
Colorado School of Mines
Computer Vision
8
Solving for E
• We have A x = 0
• This is a system of homogeneous equations
• Ignoring the trivial solution x = 0, you can find a unique
solution for x that gives the least squares solution for x; i.e.,
the solution that minimizes ∑ (p0T E p1)2
• It is proportional to the only zero eigenvalue of ATA
• You can use Singular Value Decomposition to find it:
A = U D VT
• The solution x is the column of V corresponding to the only
null singular value of A
• This is the rightmost column of V
Colorado School of Mines
Computer Vision
9
Finding E using 8-point linear algorithm
• Solve A x = 0 using Singular Value Decomposition (SVD):
A = U D VT
• The solution x is the rightmost column of V
%
%
%
%
A
Compute essential matrix E from point correspondences.
We know that p1' E p2 = 0, where p1,p2 are the normalized image coords.
We write out the equations in the unknowns E(i,j)
A x = 0
= [p1s(1,:)'.*p2s(1,:)'
p1s(1,:)'.*p2s(2,:)' p1s(1,:)' ...
p1s(2,:)'.*p2s(1,:)'
p1s(2,:)'.*p2s(2,:)' p1s(2,:)' ...
p2s(1,:)'
p2s(2,:)' ones(length(p1s),1)];
% The solution to Ax=0 is the singular vector of A corresponding to the
% smallest singular value; that is, the last column of V in A=UDV'
[U,D,V] = svd(A);
x = V(:,size(V,2));
% get last column of V
% Put unknowns into a 3x3 matrix. Transpose because Matlab's "reshape"
% uses the order E11 E21 E31 E12 ...
Escale = reshape(x,3,3)';
Colorado School of Mines
Computer Vision
10
Observations
• Results can be unstable, due to poor numerical
conditioning
• We can improve results by:
– Preconditioning: We will first translate and scale the data
points so they are centered at the origin and the average
distance to the origin is √2
– Postconditioning: The values of E are not independent.
There are only five independent parameters. E must have
rank=2 … we will enforce this
Colorado School of Mines
Computer Vision
11
Preconditioning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Scale and translate image points so that the centroid of
% the points is at the origin, and the average distance of the points to the
% origin is equal to sqrt(2).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xn = p1(1:2,:);
% xn is a 2xN matrix
N = size(xn,2);
t = (1/N) * sum(xn,2);
% this is the (x,y) centroid of the points
xnc = xn - t*ones(1,N);
% center the points; xnc is a 2xN matrix
dc = sqrt(sum(xnc.^2));
% dist of each new point to 0,0; dc is 1xN vector
davg = (1/N)*sum(dc);
% average distance to the origin
s = sqrt(2)/davg;
% the scale factor, so that avg dist is sqrt(2)
T1 = [s*eye(2), -s*t ; 0 0 1];
p1s = T1 * p1;
xn = p2(1:2,:);
% xn is a 2xN matrix
N = size(xn,2);
t = (1/N) * sum(xn,2);
% this is the (x,y) centroid of the points
xnc = xn - t*ones(1,N);
% center the points; xnc is a 2xN matrix
dc = sqrt(sum(xnc.^2));
% dist of each new point to 0,0; dc is 1xN vector
davg = (1/N)*sum(dc);
% average distance to the origin
s = sqrt(2)/davg;
% the scale factor, so that avg dist is sqrt(2)
T2 = [s*eye(2), -s*t ; 0 0 1];
p2s = T2 * p2;
Colorado School of Mines
Computer Vision
12
Postconditioning
• Enforce the property that the essential matrix has only two
non-zero eigenvalues, and that they are equal
• You can force this by taking the SVD of E
E = U S VT
• then reconstruct E with only its first two eigenvalues
E’ = U diag(1,1,0) VT
• In Matlab
[U,D,V] = svd(Escale);
Escale = U*diag([1 1 0])*V';
Colorado School of Mines
Computer Vision
13
Undoing Preconditioning
• After computing the essential matrix Escale, you then have to
adjust the result to undo the effect of point scaling. This can
be done by E = T1T Escale T2.
E = T1' * Escale * T2;
Colorado School of Mines
Computer Vision
% Undo scaling
14
Complete Code for computing Essential Matrix (1)
% Calculate the essential matrix.
Read images and corresponding
points
clear all
close all
K = [ 300 0 150;
0 300 150;
0 0
1];
% intrinsic camera parameters
% These are the points in image 1
u1 = [
61.4195 102.1798 150.0000
68.3768
124.4290 136.1955 150.0000 167.2490
1.0000
1.0000
1.0000
1.0000
106.2098
181.1490
1.0000
150.0000
197.2377
1.0000
74.3208
203.8325
1.0000
109.6134
219.1146
1.0000
150.0000
236.6025
1.0000
176.0870
127.4080
1.0000
196.1538
110.0296
1.0000
174.0000
170.7846
1.0000
192.8571
150.0000
1.0000
172.2222
207.7350
1.0000
190.0000;
184.6410;
1.0000 ];
% These are the corresponding points in image 2
u2 = [
45.5272
63.4568
86.9447
61.6620
80.2653
126.5989 136.7293 150.0000 165.9997 180.2731
1.0000
1.0000
1.0000
1.0000
1.0000
104.1468
198.5963
1.0000
75.7981
200.5196
1.0000
94.7507
217.7991
1.0000
118.6606
239.5982
1.0000
135.5451
125.7710
1.0000
176.3357
105.4355
1.0000
147.5739
172.3407
1.0000
184.5258
150.0000
1.0000
157.8633
212.1766
1.0000
191.6139;
188.5687;
1.0000 ];
I1 = imread('I1.tif');
I2 = imread('I2.tif');
% Display points on the images for visualization
imshow(I1, []);
for i=1:length(u1)
x = round(u1(1,i));
y = round(u1(2,i));
rectangle('Position', [x-4 y-4 8 8], 'EdgeColor', 'r');
text(x+4, y+4, sprintf('%d', i), 'Color', 'r');
end
figure, imshow(I2, []);
for i=1:length(u2)
x = round(u2(1,i));
y = round(u2(2,i));
rectangle('Position', [x-4 y-4 8 8], 'EdgeColor', 'r');
text(x+4, y+4, sprintf('%d', i), 'Color', 'r');
end
Colorado School of Mines
Display images and points
Computer Vision
15
Complete Code for computing Essential Matrix (2)
% Get normalized image points
p1 = inv(K)*u1;
p2 = inv(K)*u2;
Normalize points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Scale and translate image points so that the centroid of
% the points is at the origin, and the average distance of the points to the
% origin is equal to sqrt(2).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xn = p1(1:2,:);
% xn is a 2xN matrix
N = size(xn,2);
t = (1/N) * sum(xn,2);
% this is the (x,y) centroid of the points
xnc = xn - t*ones(1,N);
% center the points; xnc is a 2xN matrix
dc = sqrt(sum(xnc.^2));
% dist of each new point to 0,0; dc is 1xN vector
davg = (1/N)*sum(dc);
% average distance to the origin
s = sqrt(2)/davg;
% the scale factor, so that avg dist is sqrt(2)
T1 = [s*eye(2), -s*t ; 0 0 1];
p1s = T1 * p1;
Scale and translate points
xn = p2(1:2,:);
% xn is a 2xN matrix
N = size(xn,2);
t = (1/N) * sum(xn,2);
% this is the (x,y) centroid of the points
xnc = xn - t*ones(1,N);
% center the points; xnc is a 2xN matrix
dc = sqrt(sum(xnc.^2));
% dist of each new point to 0,0; dc is 1xN vector
davg = (1/N)*sum(dc);
% average distance to the origin
s = sqrt(2)/davg;
% the scale factor, so that avg dist is sqrt(2)
T2 = [s*eye(2), -s*t ; 0 0 1];
p2s = T2 * p2;
%
%
%
%
A
Compute essential matrix E from point correspondences.
We know that p1s' E p2s = 0, where p1s,p2s are the scaled image coords.
We write out the equations in the unknowns E(i,j)
A x = 0
= [p1s(1,:)'.*p2s(1,:)'
p1s(1,:)'.*p2s(2,:)' p1s(1,:)' ...
p1s(2,:)'.*p2s(1,:)'
p1s(2,:)'.*p2s(2,:)' p1s(2,:)' ...
p2s(1,:)'
p2s(2,:)' ones(length(p1s),1)];
Compute E
% The solution to Ax=0 is the singular vector of A corresponding to the
% smallest singular value; that is, the last column of V in A=UDV'
[U,D,V] = svd(A);
x = V(:,size(V,2));
% get last column of V
% Put unknowns into a 3x3 matrix. Transpose because Matlab's "reshape"
% uses the order E11 E21 E31 E12 ...
Escale = reshape(x,3,3)';
% Force rank=2 and equal eigenvalues
[U,D,V] = svd(Escale);
Escale = U*diag([1 1 0])*V';
Force E to have rank 2
% Undo scaling
E = T1' * Escale * T2;
Undo scaling and translation
disp('Calculated essential matrix:');
disp(E);
save E
Colorado School of Mines
Computer Vision
16
Results
• Run program “essential.m”
– This inputs the corresponding points, and calculates the essential matrix
• Verify that calculated essential matrix equals the “true” essential matrix
(to within a scale factor)
True essential matrix:
0 -1.0000
0
-0.3615
0 -3.1415
0 3.0000
0
Calculated essential matrix:
0.0001 2.4639 0.0010
0.8929 0.0834 7.7585
-0.0004 -7.3917 -0.0031
Scaled essential matrix:
-0.0000 -1.0000 -0.0004
-0.3624 -0.0338 -3.1489
0.0001 3.0000 0.0013
Colorado School of Mines
Computer Vision
17
Visualization of epipolar lines
• Draw epipolar lines to verify that corresponding points lie on
these lines
• Representation of lines
– A line (ax + by + c = 0) is represented by the homogeneous coordinates
l = (a,b,c)T
– A point p lies on the line l if and only if pT l = 0
• Epipolar lines
– Ep1 is the epipolar line in the first view corresponding to p1 in the
second view
– ETp0 is the epipolar line in the second view corresponding to p0 in the
first view
Colorado School of Mines
Computer Vision
18
Example
• Run program “drawepipolar.m”
– This inputs a pair of images, a set of corresponding points,
and an essential matrix
– It draws epipolar lines in the images
View 2
View 1
Colorado School of Mines
Computer Vision
19
Recovering Motion Parameters (advanced)
• Once you have essential matrix E, you can recover the relative
motion between cameras
• Recall that the essential matrix is made up of the translation
and rotation matrices; ie., E = [t]x R
• We can extract the translation and rotation by taking SVD of E
again, E = U D VT
• Then form the following combinations:
– t is either u3 or –u3, where u3 is the third (last) column of U
– R is either U W VT or U WT VT
• where
 0  1 0


W   1 0 0
 0 0 1


Colorado School of Mines
Computer Vision
20
Recovering Motion (continued)
• There are actually 4 possible solutions, but 3 of
them are nonsensical, meaning that they represent
situations where the scene is behind one or both
of the cameras
• Only one of the four solutions corresponds to the
case where the scene points are in front of both
cameras
– To find the correct one, we will need to reconstruct a
scene point and see if it is front of both cameras
• Remember that we can only determine motion up
to a scale factor
– The translation t will have an arbitrary magnitude; we
can only know the direction of t
– The rotation R is correct, though
Colorado School of Mines
Computer Vision
21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Extract motion parameters from essential matrix.
% We know that E = [tx] R, where
% [tx] = [ 0 -t3 t2; t3 0 -t1; -t2 t1 0]
%
% If we take SVD of E, we get E = U diag(1,1,0) V'
% t is the last column of U
[U,D,V] = svd(E);
W = [0 -1 0; 1 0 0; 0 0 1];
Hresult_c2_c1(:,:,1) = [ U*W*V'
U(:,3) ; 0
Hresult_c2_c1(:,:,2) = [ U*W*V' -U(:,3) ; 0
Hresult_c2_c1(:,:,3) = [ U*W'*V' U(:,3) ; 0
Hresult_c2_c1(:,:,4) = [ U*W'*V' -U(:,3) ; 0
0
0
0
0
0
0
0
0
1];
1];
1];
1];
% make sure each rotation component is a legal rotation matrix
for k=1:4
if det(Hresult_c2_c1(1:3,1:3,k)) < 0
Hresult_c2_c1(1:3,1:3,k) = -Hresult_c2_c1(1:3,1:3,k);
end
end
Colorado School of Mines
Computer Vision
22
True pose of
camera 2 wrt
camera 1
The four possible
calculated poses
find the correct one
by reconstructing a
point
Colorado School of Mines
H_c2_c1 =
0.9063
0
0.4226
0
0
1.0000
0
0
-0.4226
0
0.9063
0
3.0000
0
1.0000
1.0000
Calculated possible poses, camera 2 to camera 1:
(:,:,1) =
0.9063
0
-0.4226
0.9487
0
1.0000
0.0000
0
0.4226
0
0.9063
0.3162
0
0
0
1.0000
(:,:,2) =
0.9063
0
-0.4226
-0.9487
0
1.0000
0.0000
0
0.4226
0
0.9063
-0.3162
0
0
0
1.0000
(:,:,3) =
0.9786
0
0.2057
0.9487
0
-1.0000
-0.0000
0
0.2057
0
-0.9786
0.3162
0
0
0
1.0000
(:,:,4) =
0.9786
0
0.2057
-0.9487
0
-1.0000
-0.0000
0
0.2057
0
-0.9786
-0.3162
0
0
0
1.0000
Computer Vision
23
Reconstruction
• Given a hypothesized pose between the cameras, we want to reconstruct
the 3D position of a point from its image projections
• The projection of P onto the left image is
Z1 p1 = M1 P
• The projection of P onto the right image is
Z2 p2 = M2 P
• where
P
1 0 0 0


M1   0 1 0 0 
0 0 1 0


 r11 r12

M 2   r21 r22
r r
 31 32
Colorado School of Mines
r13 t x 

r23 t y  
r33 t z 
p1

2
1
R
2
t1org
p2

Computer Vision
24
Reconstruction (continued)
• Note that p1 and M1 P are parallel,
so their cross product should be zero
• Similarly for p2 and M2 P
• Point P should satisfy both
P
p1  M1P  0
p1
p2
p2  M 2P  0
• This is a system of four equations;
can solve for the three unknowns of
P (X, Y, Z) using least squares
Colorado School of Mines
Computer Vision
25
Reconstruction (continued)
• Rewrite
p1  M1P  0
p2  M 2P  0
  p1   M1 


P  0
  p 2   M 2 



• Solve the system AP=0 using SVD
p1x = [ 0
p1(3,1)
-p1(2,1)
-p1(3,1)
0
p1(1,1)
p1(2,1);
-p1(1,1);
0 ];
p2x = [ 0
p2(3,1)
-p2(2,1)
-p2(3,1)
0
p2(1,1)
p2(2,1);
-p2(1,1);
0 ];
% skew symmetric matrix
A = [ p1x * M1; p2x * M2 ];
% The solution to AP=0 is the singular vector of A corresponding to the
% smallest singular value; that is, the last column of V in A=UDV'
[U,D,V] = svd(A);
P = V(:,4);
% get last column of V
P1est = P/P(4);
% normalize
Colorado School of Mines
Computer Vision
26
Testing the 4 possibilities
•
•
Take a corresponding pair, p1 and p2
For each possible pose 21H
– Reconstruct the 3D position of the point with respect to camera 1: 1P
– Compute 3D position of the point with respect to camera 2: 2P = 12H 1P
– Test if the Z components of 1P and 2P are both > 0
for i=1:4
Hresult_c1_c2 = inv(Hresult_c2_c1(:,:,i));
M2 = Hresult_c1_c2(1:3,1:4);
A = [ p1x * M1; p2x * M2 ];
% The solution to AP=0 is the singular vector of A corresponding to the
% smallest singular value; that is, the last column of V in A=UDV'
[U,D,V] = svd(A);
P = V(:,4);
% get last column of V
P1est = P/P(4);
% normalize
P2est = Hresult_c1_c2 * P1est;
if P1est(3) > 0 & P2est(3) > 0
Hest_c2_c1 = Hresult_c2_c1(:,:,i);
% We've found a good solution
break;
% break out of for loop; can stop searching
end
end
Colorado School of Mines
Computer Vision
27
Example
• Run program “twoview.m”
– This inputs a pair of images, a set of corresponding points, and an
essential matrix
– It calculates the pose (rotation & translation) between the views
• Compare computed pose of c2 with respect to c1, to ground
truth
True pose:
H_c2_c1 =
0.9063
0 -0.4226 3.0000
0 1.0000
0
0
0.4226
0 0.9063 1.0000
0
0
0 1.0000
Reconstructed pose of camera2 wrt camera1:
0.9063 -0.0016 -0.4226 0.9487
-0.0006 1.0000 -0.0051 -0.0000
0.4226 0.0049 0.9063 0.3162
0
0
0 1.0000
Colorado School of Mines
Computer Vision
28
Reconstructing rest of points
• For each pair of corresponding image
points p1 and p2
Recall a x b = [ax]b, where
 0

a x    a3
 a
 2
– Form the skew symmetric matrices [p1x] and
[p2x]
– Form the matrix equation
– where
1 0 0 0


M1   0 1 0 0 
0 0 1 0


 a3
0
a2 

 a1 
0 
a1
 p1  M1 

P  0
  p 2  M 2 
 r11 r12

M 2   r21 r22
r r
 31 32
r13 t x 

r23 t y  
r33 t z 

2
1
R
2
t1org

– solve for P using SVD
Colorado School of Mines
Computer Vision
29
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Hest_c1_c2 = inv(Hest_c2_c1);
M2est = Hest_c1_c2(1:3,:);
% Reconstruct point positions (these are good to the same scale factor)
fprintf('Reconstructed points wrt camera1:\n');
for i=1:length(p1)
p1x = [ 0
-p1(3,i)
p1(2,i);
p1(3,i)
0
-p1(1,i);
-p1(2,i)
p1(1,i)
0 ];
p2x = [ 0
-p2(3,i)
p2(2,i);
p2(3,i)
0
-p2(1,i);
-p2(2,i)
p2(1,i)
0 ];
A = [ p1x * M1; p2x * M2est ];
[U,D,V] = svd(A);
P = V(:,4);
P1est(:,i) = P/P(4);
% get last column of V
% normalize
fprintf('%f %f %f\n', P1est(1,i), P1est(2,i),P1est(3,i));
end
% Show the reconstruction result in 3D
figure;
plot3(P1est(1,:),P1est(2,:),P1est(3,:),'d');
axis equal;
axis vis3d;
Colorado School of Mines
Computer Vision
30
Example
Reconstructed points wrt camera 1
Reconstructed points wrt camera1:
-0.547790 -0.158133 1.855227
-0.273890 -0.079065 1.718247
0.000000 0.000000 1.581274
-0.547793 0.115762 2.013372
-0.273892 0.194825 1.876391
0.000000 0.273887 1.739416
-0.547797 0.389660 2.171521
-0.273893 0.468720 2.034536
0.000000 0.547780 1.897560
0.158129 -0.136944 1.818480
0.316261 -0.273893 2.055698
0.158130 0.136945 1.976628
0.316264 0.000000 2.213851
0.158132 0.410840 2.134777
0.316267 0.273898 2.372005
Colorado School of Mines
True 3D locations of points wrt camera 1
-1.7321
-0.8660
0
-1.7321
-0.8660
0
-1.7321
-0.8660
0
0.5000
1.0000
0.5000
1.0000
0.5000
1.0000
Computer Vision
-0.5000
-0.2500
0
0.3660
0.6160
0.8660
1.2321
1.4821
1.7321
-0.4330
-0.8660
0.4330
0.0000
1.2990
0.8660
5.8660
5.4330
5.0000
6.3660
5.9330
5.5000
6.8660
6.4330
6.0000
5.7500
6.5000
6.2500
7.0000
6.7500
7.5000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
1.0000
31