Skip to content

Commit 47a6669

Browse files
SergioRAgostinhoSergioRAgostinho
committed
files were left out of the commit for release 0.9
1 parent 24e94a7 commit 47a6669

5 files changed

Lines changed: 199 additions & 0 deletions

File tree

‎aux/cross_vec3.m‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
function out = cross_vec3(u, v)
2+
%CROSS_VEC3 Function to compute the cross product of two 3D vectors. The
3+
% function disregards if they're row or collumn vectors. The default
4+
% MATLAB implementation is simply too slow.
5+
%
6+
% Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com
7+
% Date: Mar 2015
8+
% Version: 0.9
9+
% Repo: https://github.com/SergioRAgostinho/five_point_algorithm
10+
% Feel free to provide feedback or contribute.
11+
12+
if numel(u) ~= 3
13+
error('cross_vec3:wrong_dimensions','u must be either 1x3 ou 3x1');
14+
end
15+
16+
if numel(v) ~= 3
17+
error('cross_vec3:wrong_dimensions','v must be either 1x3 ou 3x1');
18+
end
19+
20+
out = [ u(2)*v(3) - u(3)*v(2);
21+
u(3)*v(1) - u(1)*v(3);
22+
u(1)*v(2) - u(2)*v(1)];
23+
end

‎aux/cube_3D.m‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
function cube_pts = cube_3D(centre_3D, size)
2+
%CUBE_3D Auxiliary function that given a cube centre coordinates and the
3+
% edge size returns the 3D coordenates of all vertices in a 3x8 matrix.
4+
%
5+
% Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com
6+
% Date: Mar 2015
7+
% Version: 0.9
8+
% Repo: https://github.com/SergioRAgostinho/five_point_algorithm
9+
% Feel free to provide feedback or contribute.
10+
11+
if numel(centre_3D) ~= 3
12+
error('cube_3D:wrong_dimensions:centre_3D',['centre_3D needs to either be a 3x1 collum ' ...
13+
'vector or a 1x3 row vector']);
14+
end
15+
16+
if numel(size) ~= 1
17+
error('cube_3D:wrong_dimensions:size', 'size is a scalar');
18+
end
19+
20+
x = centre_3D(1);
21+
y = centre_3D(2);
22+
z = centre_3D(3);
23+
hs = size/2;
24+
25+
cube_pts = [x + hs, x + hs, x - hs, x - hs, x + hs, x + hs, x - hs, x - hs; ...
26+
y + hs, y - hs, y + hs, y - hs, y + hs, y - hs, y + hs, y - hs; ...
27+
z + hs, z + hs, z + hs, z + hs, z - hs, z - hs, z - hs, z - hs];
28+
end

‎aux/frustrum.m‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
function h_out = frustrum(R,t,h)
2+
% Work in progress
3+
%
4+
% Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com
5+
% Date: Mar 2015
6+
% Version: 0.9
7+
% Repo: https://github.com/SergioRAgostinho/five_point_algorithm
8+
% Feel free to provide feedback or contribute.
9+
10+
T = [R t; zeros(1,3) 1];
11+
R_view = [0 0 1 0; -1 0 0 0 ; 0 -1 0 0; 0 0 0 1];
12+
frame = [1 0 0 0;
13+
0 1 0 0;
14+
0 0 1 0;
15+
1 1 1 1];
16+
17+
18+
pts = R_view *(T \ frame);
19+
20+
21+
if nargin > 2 && ishandle(h)
22+
next_plot = get(h, 'NextPlot');
23+
set(h, 'NextPlot', 'add');
24+
25+
h_p = plot3(h, [pts(1,4) pts(1,1)], [pts(2,4) pts(2,1)], [pts(3,4) pts(3,1)], 'r', ...
26+
[pts(1,4) pts(1,2)], [pts(2,4) pts(2,2)], [pts(3,4) pts(3,2)], 'g', ...
27+
[pts(1,4) pts(1,3)], [pts(2,4) pts(2,3)], [pts(3,4) pts(3,3)], 'b');
28+
set(h, 'NextPlot', next_plot);
29+
else
30+
h_p = plot3([pts(1,4) pts(1,1)], [pts(2,4) pts(2,1)], [pts(3,4) pts(3,1)], 'r', ...
31+
[pts(1,4) pts(1,2)], [pts(2,4) pts(2,2)], [pts(3,4) pts(3,2)], 'g', ...
32+
[pts(1,4) pts(1,3)], [pts(2,4) pts(2,3)], [pts(3,4) pts(3,3)], 'b');
33+
end
34+
35+
h_out = get(h_p(1), 'Parent');
36+
37+
end

‎aux/triangulate.m‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
function pts = triangulate(pts1, pts2, K1, K2, E, R, t)
2+
%TRIANGULATE Given 2D point matches between two images and the
3+
% transformatiom between them in terms of the rotation matrix R and the
4+
% translation vector t, such a point in camera 1 reference frame, can be
5+
% transformed to camera 2 reference frame through, P_2 = [R t; zeros(1,3) 1]
6+
% This triangulation method assumes ideal point correspondence and it is
7+
% presented in "An Efficient Solution to the Five-Point Relative Pose
8+
% Problem" by David Nister.
9+
% DOI: http://dx.doi.org/10.1109/TPAMI.2004.17
10+
%
11+
% Known Issues:
12+
% - There's no need to request E and R|t
13+
%
14+
% TODO:
15+
% - Fix the required arguments.
16+
%
17+
% Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com
18+
% Date: Mar 2015
19+
% Version: 0.9
20+
% Repo: https://github.com/SergioRAgostinho/five_point_algorithm
21+
% Feel free to provide feedback or contribute.
22+
23+
if size(pts1,1) ~= 2 || size(pts2,1) ~= 2
24+
error('triangulate:wrong_dimensions','pts1 and pts2 must be of size 2xn');
25+
end
26+
27+
if size(pts1,2) ~= size(pts2,2)
28+
error('triangulate:wrong_dimensions','pts1 and pts2 must have the same number of points');
29+
end
30+
31+
if ~all(size(R) == [3, 3])
32+
error('triangulate:wrong_dimensions','R must be of size 3x3');
33+
end
34+
35+
if ~all(size(t) == [3, 1])
36+
error('triangulate:wrong_dimensions','t must be of size 3x1');
37+
end
38+
39+
n = size(pts1,2);
40+
pts = zeros(4,n);
41+
q_1 = K1 \ [pts1; ones(1,n)];
42+
q_2 = K2 \ [pts2; ones(1,n)];
43+
44+
for m = 1:n
45+
a = E'*q_2(:,m);
46+
b = cross_vec3(q_1(:,m), [a(1:2); 0]);
47+
c = cross_vec3(q_2(:,m), diag([1 1 0])*E*q_1(:,m));
48+
d = cross_vec3(a, b);
49+
50+
P = [R t];
51+
C = P'*c;
52+
Q = [d*C(4); -d(1:3)'*C(1:3)];
53+
54+
pts(:,m) = Q;
55+
end
56+
57+
end

‎examples/ex_01_full_synthetic.m‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
% FIVE_POINT_ALGORITHM -
2+
% Example 1: Full synthetic
3+
%
4+
% Draw a fictional cube whose vertices are seen by two camera. Project
5+
% these points into the camera image plain, run the five_point_method on
6+
% top of them and verify the final result.
7+
%
8+
% Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com
9+
% Date: Mar 2015
10+
% Version: 0.9
11+
% Repo: https://github.com/SergioRAgostinho/five_point_algorithm
12+
% Feel free to provide feedback or contribute.
13+
14+
[path, ~, ~] = fileparts(which('ex_01_full_synthetic'));
15+
addpath([path '/..']);
16+
addpath([path '/../aux']);
17+
clear path
18+
19+
cube_pts = cube_3D([0 0 10]', 0.2);
20+
cube_pts_homo = [cube_pts; ones(1,size(cube_pts,2))];
21+
22+
K1 = [602.5277 0 177.3328;
23+
0 562.9129 102.8893;
24+
0 0 1.0000];
25+
26+
K2 = [562.9129 0 102.8893;
27+
0 602.5277 177.3328;
28+
0 0 1.0000];
29+
30+
%The first camera will be placed at the origin
31+
P_1 = [eye(3), zeros(3,1)];
32+
cam1_pts = K1*P_1*cube_pts_homo;
33+
pts1 = cam1_pts(1:2,:)./(ones(2,1)*cam1_pts(3,:));
34+
35+
%The second camera will be displaced 1 unit (to match our algorithm
36+
%assumption) in the +x direction of the first camera reference frame and
37+
%rotated -10 degrees in y.
38+
t_o = [1; 0; 0];
39+
R_o = [ cosd(-10) 0 sind(-10);
40+
0 1 0;
41+
-sind(-10) 0 cosd(-10)];
42+
P_2 = [R_o', -R_o'*t_o];
43+
cam2_pts = K2*P_2*cube_pts_homo;
44+
pts2 = cam2_pts(1:2,:)./(ones(2,1)*cam2_pts(3,:));
45+
46+
[E, R, t, Eo] = five_point_algorithm(pts1(:,1:5), pts2(:,1:5), K1, K2);
47+
48+
%In this case the first solution is the correct one.
49+
pts3Dhomo = triangulate(pts1, pts2, K1, K2, E{1}, R{1}, t{1});
50+
pts3D = pts3Dhomo(1:3,:)./(ones(3,1)*pts3Dhomo(4,:));
51+
52+
%Display results
53+
cube_pts
54+
pts3D

0 commit comments

Comments
 (0)