├── LICENSE ├── README.md ├── aux ├── cross_vec3.m ├── cube_3D.m ├── frustrum.m └── triangulate.m ├── examples ├── ex_01_full_synthetic.m └── ex_02_minimal.m └── five_point_algorithm.m /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Sérgio Agostinho 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Five-Point Algorithm 2 | A MATLAB implementation of the Five-Point Algorithm by David Nistér 3 | 4 | Given five points matches between two images, and the intrinsic parameters of each camera. Estimate the essential matrix `E`, the rotation matrix `R` and translation vector `t`, between both images. This algorithm is based on the method described by David Nistér in ["An Efficient Solution to the Five-Point Relative Pose Problem"](http://dx.doi.org/10.1109/TPAMI.2004.17) 5 | 6 | `E_all = FIVE_POINT_ALGORITHM(pts1, pts2, K1, K2)` returns in `E` all the valid essential matrix solutions for the five point correspondence. If you don't need `R` and `t`, use this version as it avoids computing unnecessary results. 7 | 8 | `[E_all, R_all, t_all, Eo_all] = FIVE_POINT_ALGORITHM(pts1, pts2, K1, K2)` also returns in `R_all` and `t_all` all the rotation matrices and translation vectors of camera 2 for the different essential matrices, such that a 3D point in camera 1 reference frame can be transformed into the camera 2 reference frame through `p_2 = R{n}*p_1 + t{n}`. `Eo_all` is the essential matrix before the imposing the structure `U*diag([1 1 0])*V'`. It should help get a better feeling on the accuracy of the solution. All these return values a nx1 cell arrays. 9 | 10 | #### Arguments: 11 | 12 | `pts1`, `pts2` - assumed to have dimension 2x5 and of equal size. 13 | 14 | `K1`, `K2` - 3x3 intrinsic parameters of cameras 1 and 2 respectively 15 | 16 | #### Known Issues: 17 | 18 | - R and t computation is done assuming perfect point correspondence. 19 | 20 | #### TODO: 21 | - [x] Extract `R` and `t` from `E` 22 | - [x] Provide example cases. 23 | - [ ] Extract `R` and `t` without perfect point correspondence 24 | - [ ] Augment example cases. 25 | - [ ] Implement the variant with 5 points over 3 images 26 | - [ ] Handle more than 5 points 27 | 28 | #### Other Info 29 | * Author: Sérgio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com 30 | * Date: Feb 2015 31 | * Last modified: Mar 2015 32 | * Version: 0.9 33 | 34 | Feel free to provide feedback or contribute. 35 | -------------------------------------------------------------------------------- /aux/cross_vec3.m: -------------------------------------------------------------------------------- 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: -------------------------------------------------------------------------------- 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: -------------------------------------------------------------------------------- 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: -------------------------------------------------------------------------------- 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: -------------------------------------------------------------------------------- 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 55 | -------------------------------------------------------------------------------- /examples/ex_02_minimal.m: -------------------------------------------------------------------------------- 1 | % FIVE_POINT_ALGORITHM - 2 | % Example 2: minimal example 3 | % 4 | % Work in Progress - may have bugs 5 | % 6 | % Given point matches extracted from two pictures and taken with the 7 | % same camere with known intrinsic parameters, exemplify the use of 8 | % five_point_algorithm 9 | % 10 | % Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com 11 | % Date: Feb 2015 12 | % Last modified: Mar 2015 13 | % Version: 0.9 14 | % Repo: https://github.com/SergioRAgostinho/five_point_algorithm 15 | % Feel free to provide feedback or contribute. 16 | 17 | [path, ~, ~] = fileparts(which('ex_02_minimal')); 18 | addpath([path '/..']); 19 | addpath([path '/../aux']); 20 | clear path 21 | 22 | pts1 = [288.8398 12.1534 317.5059 74.5754 44.1327; 23 | 77.2382 163.7803 82.8476 220.5643 192.9634]; 24 | 25 | pts2 = [286.1892 6.9846 312.5673 70.0283 40.2131; 26 | 76.7289 164.3921 81.3119 220.4840 194.1166]; 27 | 28 | K = [602.5277 0 177.3328; 29 | 0 562.9129 102.8893; 30 | 0 0 1.0000]; 31 | 32 | 33 | [E, R, t, Eo] = five_point_algorithm(pts1, pts2, K, K); 34 | 35 | pts3Dhomo = triangulate(pts1, pts2, K, K, E{1}, R{1}, t{1}); 36 | pts3D = pts3Dhomo(1:3,:)./(ones(3,1)*pts3Dhomo(4,:)); 37 | 38 | -------------------------------------------------------------------------------- /five_point_algorithm.m: -------------------------------------------------------------------------------- 1 | function [E_all, R_all, t_all, Eo_all] = five_point_algorithm( pts1, pts2, K1, K2 ) 2 | %FIVE_POINT_ALGORITHM Given five points matches between two images, and the 3 | % intrinsic parameters of each camera. Estimate the essential matrix E, the 4 | % rotation matrix R and translation vector t, between both images. This 5 | % algorithm is based on the method described by David Nister in "An 6 | % Efficient Solution to the Five-Point Relative Pose Problem" 7 | % DOI: http://dx.doi.org/10.1109/TPAMI.2004.17 8 | % 9 | % E = FIVE_POINT_ALGORITHM(pts1, pts2, K1, K2) returns in E all the valid 10 | % Essential matrix solutions for the five point correspondence. If you 11 | % don't need the R and t, use this version as it avoids computing 12 | % unnecessary results. 13 | % 14 | % [E_all, R_all, t_all, Eo_all] = FIVE_POINT_ALGORITHM(pts1, pts2, K1, K2) 15 | % also returns in R_all and t_all all the rotation matrices and translation 16 | % vectors of camera 2 for the different essential matrices, such that a 3D 17 | % point in camera 1 reference frame can be transformed into the camera 2 18 | % reference frame through p_2 = R{n}*p_1 + t{n}. Eo_all is the essential 19 | % matrix before the imposing the structure U*diag([1 1 0])*V'. It should 20 | % help get a better feeling on the accuracy of the solution. All these 21 | % return values a nx1 cell arrays. 22 | % 23 | % 24 | % Arguments: 25 | % pts1, pts2 - assumed to have dimension 2x5 and of equal size. 26 | % K1, K2 - 3x3 intrinsic parameters of cameras 1 and 2 respectively 27 | % 28 | % Know Issues: 29 | % - R and t computation is done assuming perfect point correspondence. 30 | % 31 | % TODO: 32 | % - Extract R and t without perfect point correspondence 33 | % - Augment example cases. 34 | % - Implement the variant with 5 points over 3 images 35 | % - Handle more than 5 points 36 | % 37 | % Author: Sergio Agostinho - sergio(dot)r(dot)agostinho(at)gmail(dot)com 38 | % Date: Feb 2015 39 | % Last modified: Mar 2015 40 | % Version: 0.9 41 | % Repo: https://github.com/SergioRAgostinho/five_point_algorithm 42 | % Feel free to provide feedback or contribute. 43 | 44 | if ~all(size(pts1) == [2,5]) || ~all(size(pts2) == [2,5]) 45 | error('five_point_algorithm:wrong_dimensions','pts1 and pts2 must be of size 2x5'); 46 | end 47 | 48 | if ~all(size(K1) == [3, 3]) || ~all(size(K2) == [3, 3]) 49 | error('five_point_algorithm:wrong_dimensions','K1 and K2 must be of size 3x3'); 50 | end 51 | 52 | N = 5; 53 | q1 = K1 \ [pts1; ones(1,N)]; 54 | q2 = K2 \ [pts2; ones(1,N)]; 55 | 56 | q = [q1(1,:)'.* q2(1,:)', q1(2,:)'.* q2(1,:)', q1(3,:)'.* q2(1,:)', ... 57 | q1(1,:)'.* q2(2,:)', q1(2,:)'.* q2(2,:)', q1(3,:)'.* q2(2,:)', ... 58 | q1(1,:)'.* q2(3,:)', q1(2,:)'.* q2(3,:)', q1(3,:)'.* q2(3,:)']; 59 | 60 | %according to the author, the null space step can be further optimized, 61 | %following the efficiency considerations in section 3.2.1 62 | % Can be further expand it to N > 5 by extracting the four singular vectors 63 | % corresponding to the four smallest singular values. 64 | nullSpace = null(q); 65 | X = nullSpace(:,1); 66 | Y = nullSpace(:,2); 67 | Z = nullSpace(:,3); 68 | W = nullSpace(:,4); 69 | 70 | % populating the equation system 71 | mask = [1,2,3;4,5,6;7,8,9]; 72 | Xmat = X(mask); 73 | Ymat = Y(mask); 74 | Zmat = Z(mask); 75 | Wmat = W(mask); 76 | X_ = (K2') \ Xmat / K1; 77 | Y_ = (K2') \ Ymat / K1; 78 | Z_ = (K2') \ Zmat / K1; 79 | W_ = (K2') \ Wmat / K1; 80 | 81 | %det(F) 82 | detF = p2p1(p1p1([X_(1,2),Y_(1,2),Z_(1,2),W_(1,2)], ... 83 | [X_(2,3),Y_(2,3),Z_(2,3),W_(2,3)]) ... 84 | - p1p1([X_(1,3),Y_(1,3),Z_(1,3),W_(1,3)], ... 85 | [X_(2,2),Y_(2,2),Z_(2,2),W_(2,2)]),... 86 | [X_(3,1),Y_(3,1),Z_(3,1),W_(3,1)]) + ... 87 | p2p1(p1p1([X_(1,3),Y_(1,3),Z_(1,3),W_(1,3)], ... 88 | [X_(2,1),Y_(2,1),Z_(2,1),W_(2,1)]) ... 89 | - p1p1([X_(1,1),Y_(1,1),Z_(1,1),W_(1,1)], ... 90 | [X_(2,3),Y_(2,3),Z_(2,3),W_(2,3)]),... 91 | [X_(3,2),Y_(3,2),Z_(3,2),W_(3,2)]) + ... 92 | p2p1(p1p1([X_(1,1),Y_(1,1),Z_(1,1),W_(1,1)], ... 93 | [X_(2,2),Y_(2,2),Z_(2,2),W_(2,2)]) ... 94 | - p1p1([X_(1,2),Y_(1,2),Z_(1,2),W_(1,2)], ... 95 | [X_(2,1),Y_(2,1),Z_(2,1),W_(2,1)]),... 96 | [X_(3,3),Y_(3,3),Z_(3,3),W_(3,3)]); 97 | 98 | %Flipped V 99 | EE_t11 = p1p1([Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)], ... 100 | [Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)]) + ... 101 | p1p1([Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)], ... 102 | [Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)]) + ... 103 | p1p1([Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)], ... 104 | [Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)]); 105 | EE_t12 = p1p1([Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)], ... 106 | [Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)]) + ... 107 | p1p1([Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)], ... 108 | [Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)]) + ... 109 | p1p1([Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)], ... 110 | [Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)]); 111 | EE_t13 = p1p1([Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)], ... 112 | [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]) + ... 113 | p1p1([Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)], ... 114 | [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]) + ... 115 | p1p1([Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)], ... 116 | [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 117 | EE_t22 = p1p1([Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)], ... 118 | [Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)]) + ... 119 | p1p1([Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)], ... 120 | [Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)]) + ... 121 | p1p1([Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)], ... 122 | [Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)]); 123 | EE_t23 = p1p1([Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)], ... 124 | [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]) + ... 125 | p1p1([Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)], ... 126 | [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]) + ... 127 | p1p1([Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)], ... 128 | [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 129 | EE_t33 = p1p1([Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)], ... 130 | [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]) + ... 131 | p1p1([Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)], ... 132 | [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]) + ... 133 | p1p1([Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)], ... 134 | [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 135 | % Not used 136 | % EE_t21 = EE_t12; 137 | % EE_t31 = EE_t13; 138 | % EE_t32 = EE_t23; 139 | 140 | A_11 = EE_t11 - 0.5*(EE_t11 + EE_t22 + EE_t33); 141 | A_12 = EE_t12; 142 | A_13 = EE_t13; 143 | A_21 = A_12; 144 | A_22 = EE_t22 - 0.5*(EE_t11 + EE_t22 + EE_t33); 145 | A_23 = EE_t23; 146 | A_31 = A_13; 147 | A_32 = A_23; 148 | A_33 = EE_t33 - 0.5*(EE_t11 + EE_t22 + EE_t33); 149 | 150 | AE_11 = p2p1(A_11, [Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)]) + ... 151 | p2p1(A_12, [Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)]) + ... 152 | p2p1(A_13, [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]); 153 | AE_12 = p2p1(A_11, [Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)]) + ... 154 | p2p1(A_12, [Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)]) + ... 155 | p2p1(A_13, [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]); 156 | AE_13 = p2p1(A_11, [Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)]) + ... 157 | p2p1(A_12, [Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)]) + ... 158 | p2p1(A_13, [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 159 | AE_21 = p2p1(A_21, [Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)]) + ... 160 | p2p1(A_22, [Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)]) + ... 161 | p2p1(A_23, [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]); 162 | AE_22 = p2p1(A_21, [Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)]) + ... 163 | p2p1(A_22, [Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)]) + ... 164 | p2p1(A_23, [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]); 165 | AE_23 = p2p1(A_21, [Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)]) + ... 166 | p2p1(A_22, [Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)]) + ... 167 | p2p1(A_23, [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 168 | AE_31 = p2p1(A_31, [Xmat(1,1),Ymat(1,1),Zmat(1,1),Wmat(1,1)]) + ... 169 | p2p1(A_32, [Xmat(2,1),Ymat(2,1),Zmat(2,1),Wmat(2,1)]) + ... 170 | p2p1(A_33, [Xmat(3,1),Ymat(3,1),Zmat(3,1),Wmat(3,1)]); 171 | AE_32 = p2p1(A_31, [Xmat(1,2),Ymat(1,2),Zmat(1,2),Wmat(1,2)]) + ... 172 | p2p1(A_32, [Xmat(2,2),Ymat(2,2),Zmat(2,2),Wmat(2,2)]) + ... 173 | p2p1(A_33, [Xmat(3,2),Ymat(3,2),Zmat(3,2),Wmat(3,2)]); 174 | AE_33 = p2p1(A_31, [Xmat(1,3),Ymat(1,3),Zmat(1,3),Wmat(1,3)]) + ... 175 | p2p1(A_32, [Xmat(2,3),Ymat(2,3),Zmat(2,3),Wmat(2,3)]) + ... 176 | p2p1(A_33, [Xmat(3,3),Ymat(3,3),Zmat(3,3),Wmat(3,3)]); 177 | 178 | % Group and permute the collumns of our polynomial vectors to prepare for 179 | % the Gaussian Jordan elimination with partial pivoting 180 | % Previously our 3rd order polynomial vector arrangement was like this 181 | % x^3 | y^3 | z^3 | x^2y | xy^2 | x^2z | xz^2 | y^2z | yz^2 | xyz 182 | % x^2 | y^2 | z^2 | xy | xz | yz | x | y | z | 1 183 | % and now we are going to need this 184 | % x^3 | y^3 | x^2y | xy^2 | x^2z | x^2 | y^2z | y^2 | xyz | xy 185 | % xz^2 | xz | x | yz^2 | yz | y | z^3 | z^2 | z | 1 186 | 187 | A = [detF; AE_11; AE_12; AE_13; AE_21; AE_22; AE_23; AE_31; AE_32; AE_33]; 188 | A = A(:,[1,2,4,5,6,11,8,12,10,14,7,15,17,9,16,18,3,13,19,20]); 189 | 190 | % Gauss Jordan elimination (partial pivoting after) 191 | A_el = gj_elim_pp(A); 192 | 193 | % Subtraction and forming matrix B 194 | k_row = partial_subtrc(A_el(5,11:20), A_el(6,11:20)); 195 | l_row = partial_subtrc(A_el(7,11:20), A_el(8,11:20)); 196 | m_row = partial_subtrc(A_el(9,11:20), A_el(10,11:20)); 197 | 198 | B_11 = k_row(1,1:4); 199 | B_12 = k_row(1,5:8); 200 | B_13 = k_row(1,9:13); 201 | B_21 = l_row(1,1:4); 202 | B_22 = l_row(1,5:8); 203 | B_23 = l_row(1,9:13); 204 | B_31 = m_row(1,1:4); 205 | B_32 = m_row(1,5:8); 206 | B_33 = m_row(1,9:13); 207 | 208 | p_1 = pz4pz3(B_23, B_12) - pz4pz3(B_13, B_22); 209 | p_2 = pz4pz3(B_13, B_21) - pz4pz3(B_23, B_11); 210 | p_3 = pz3pz3(B_11, B_22) - pz3pz3(B_12, B_21); 211 | 212 | n_row = pz7pz3(p_1, B_31) + pz7pz3(p_2, B_32) + pz6pz4(p_3, B_33); 213 | 214 | %Extracting roots from n_row using companion matrix eigen values 215 | n_row_scaled = n_row/n_row(1); 216 | 217 | e_val = eig([-n_row_scaled(2:end); 218 | eye(9), zeros(9,1)]); 219 | 220 | 221 | m = 0; 222 | for n = 1:10 223 | if ~isreal(e_val(n)) 224 | continue 225 | end 226 | 227 | m = m + 1; 228 | end 229 | 230 | R_all = cell(m,1); 231 | t_all = cell(m,1); 232 | E_all = cell(m,1); 233 | Eo_all = cell(m,1); 234 | 235 | m = 1; 236 | for n = 1:10 237 | if ~isreal(e_val(n)) 238 | continue 239 | end 240 | z = e_val(n); 241 | 242 | %Backsubstition 243 | p_z6 = [z^6; z^5; z^4; z^3; z^2; z; 1]; 244 | p_z7 = [z^7; p_z6]; 245 | 246 | x = (p_1*p_z7)/(p_3*p_z6); 247 | y = (p_2*p_z7)/(p_3*p_z6); 248 | 249 | Eo = x*Xmat + y*Ymat + z*Zmat + Wmat; 250 | Eo_all{m} = Eo; 251 | [U,~,V] = svd(Eo); 252 | 253 | 254 | E = U*diag([1 1 0])*V'; 255 | E_all{m} = E; 256 | 257 | %stop here if nothing else is required to be computed 258 | if nargout < 2 259 | m = m + 1; 260 | continue 261 | end 262 | 263 | %check determinan signs 264 | if(det(U) < 0) 265 | U(:,3) = -U(:,3); 266 | end 267 | 268 | if (det(V) < 0) 269 | V(:,3) = -V(:,3); 270 | end 271 | 272 | %Extracting R and t from E 273 | D = [0 1 0; 274 | -1 0 0; 275 | 0 0 1]; 276 | 277 | q_1 = q1(:,1); 278 | q_2 = q2(:,1); 279 | 280 | 281 | for n = 1:4 282 | switch(n) 283 | case 1 284 | t = U(:,3); 285 | R = U*D*V'; 286 | case 2 287 | t = -U(:,3); 288 | R = U*D*V'; 289 | case 3 290 | t = U(:,3); 291 | R = U*D'*V'; 292 | case 4 293 | t = -U(:,3); 294 | R = U*D'*V'; 295 | end 296 | 297 | %Cheirality (points in front of the camera) constraint assuming perfect 298 | %point correspondence 299 | a = E'*q_2; 300 | b = cross_vec3(q_1, [a(1:2); 0]); 301 | c = cross_vec3(q_2, diag([1 1 0])*E*q_1); 302 | d = cross_vec3(a, b); 303 | 304 | P = [R t]; 305 | C = P'*c; 306 | Q = [d*C(4); -d(1:3)'*C(1:3)]; 307 | 308 | %Cheirality test 309 | 310 | %behind the 1st camera 311 | if (Q(3)*Q(4) < 0) 312 | continue 313 | end 314 | 315 | %behind the 2nd camera 316 | c_2 = P*Q; 317 | if (c_2(3)*Q(4) < 0) 318 | continue 319 | end 320 | 321 | R_all{m} = R; 322 | t_all{m} = t; 323 | break 324 | end 325 | m = m + 1; 326 | end 327 | 328 | end 329 | 330 | function out = cross_vec3(u, v) 331 | %CROSS_VEC Function to compute the cross product of two 3D column vectors. 332 | %The default MATLAB implementation is simply too slow. 333 | 334 | out = [ u(2)*v(3) - u(3)*v(2); 335 | u(3)*v(1) - u(1)*v(3); 336 | u(1)*v(2) - u(2)*v(1)]; 337 | end 338 | 339 | function po = pz6pz4(p1, p2) 340 | %PZ4PZ3 Function responsible for multiplying a 6th order z polynomial p1 341 | % by a 4th order z polynomial p2 342 | % p1 - Is a row vector arranged like: z6 | z5 | z4 | z3 | z2 | z | 1 343 | % p2 - Is a row vector arranged like: z4 | z3 | z2 | z | 1 344 | % po - Is a row vector arranged like: 345 | % z10 | z9 | z8 | z7 | z6 | z5 | z4 | z3 | z2 | z | 1 346 | 347 | po = [ p1(1)*p2(1), ... z10 348 | p1(2)*p2(1) + p1(1)*p2(2), ... z9 349 | p1(3)*p2(1) + p1(2)*p2(2) + p1(1)*p2(3), ... z8 350 | p1(4)*p2(1) + p1(3)*p2(2) + p1(2)*p2(3) + p1(1)*p2(4), ... z7 351 | p1(5)*p2(1) + p1(4)*p2(2) + p1(3)*p2(3) + p1(2)*p2(4) + p1(1)*p2(5), ... z6 352 | p1(6)*p2(1) + p1(5)*p2(2) + p1(4)*p2(3) + p1(3)*p2(4) + p1(2)*p2(5), ... z5 353 | p1(7)*p2(1) + p1(6)*p2(2) + p1(5)*p2(3) + p1(4)*p2(4) + p1(3)*p2(5), ... z4 354 | p1(7)*p2(2) + p1(6)*p2(3) + p1(5)*p2(4) + p1(4)*p2(5), ... z3 355 | p1(7)*p2(3) + p1(6)*p2(4) + p1(5)*p2(5), ... z2 356 | p1(7)*p2(4) + p1(6)*p2(5), ... z 357 | p1(7)*p2(5)]; % 1 358 | end 359 | 360 | function po = pz7pz3(p1, p2) 361 | %PZ4PZ3 Function responsible for multiplying a 7th order z polynomial p1 362 | % by a 3rd order z polynomial p2 363 | % p1 - Is a row vector arranged like: z7 | z6 | z5 | z4 | z3 | z2 | z | 1 364 | % p2 - Is a row vector arranged like: z3 | z2 | z | 1 365 | % po - Is a row vector arranged like: 366 | % z10 | z9 | z8 | z7 | z6 | z5 | z4 | z3 | z2 | z | 1 367 | 368 | po = [ p1(1)*p2(1), ... z10 369 | p1(2)*p2(1) + p1(1)*p2(2), ... z9 370 | p1(3)*p2(1) + p1(2)*p2(2) + p1(1)*p2(3), ... z8 371 | p1(4)*p2(1) + p1(3)*p2(2) + p1(2)*p2(3) + p1(1)*p2(4), ... z7 372 | p1(5)*p2(1) + p1(4)*p2(2) + p1(3)*p2(3) + p1(2)*p2(4), ... z6 373 | p1(6)*p2(1) + p1(5)*p2(2) + p1(4)*p2(3) + p1(3)*p2(4), ... z5 374 | p1(7)*p2(1) + p1(6)*p2(2) + p1(5)*p2(3) + p1(4)*p2(4), ... z4 375 | p1(8)*p2(1) + p1(7)*p2(2) + p1(6)*p2(3) + p1(5)*p2(4), ... z3 376 | p1(8)*p2(2) + p1(7)*p2(3) + p1(6)*p2(4), ... z2 377 | p1(8)*p2(3) + p1(7)*p2(4), ... z 378 | p1(8)*p2(4)]; % 1 379 | end 380 | 381 | function po = pz4pz3(p1, p2) 382 | %PZ4PZ3 Function responsible for multiplying a 4th order z polynomial p1 383 | % by a 3rd order z polynomial p2 384 | % p1 - Is a row vector arranged like: z4 | z3 | z2 | z | 1 385 | % p2 - Is a row vector arranged like: z3 | z2 | z | 1 386 | % po - Is a row vector arranged like: z7 | z6 | z5 | z4 | z3 | z2 | z | 1 387 | 388 | po = [ p1(1)*p2(1), ... z7 389 | p1(2)*p2(1) + p1(1)*p2(2), ... z6 390 | p1(3)*p2(1) + p1(2)*p2(2) + p1(1)*p2(3), ... z5 391 | p1(4)*p2(1) + p1(3)*p2(2) + p1(2)*p2(3) + p1(1)*p2(4), ... z4 392 | p1(5)*p2(1) + p1(4)*p2(2) + p1(3)*p2(3) + p1(2)*p2(4), ... z3 393 | p1(5)*p2(2) + p1(4)*p2(3) + p1(3)*p2(4), ... z2 394 | p1(5)*p2(3) + p1(4)*p2(4), ... z 395 | p1(5)*p2(4)]; % 1 396 | end 397 | 398 | function po = pz3pz3(p1, p2) 399 | %PZ3PZ3 Function responsible for multiplying two 3rd order z polynomial 400 | % p1, p2 - Are row vector arranged like: z3 | z2 | z | 1 401 | % po - Is a row vector arranged like: z6 | z5 | z4 | z3 | z2 | z | 1 402 | 403 | po = [ p1(1)*p2(1), ... z6 404 | p1(1)*p2(2) + p1(2)*p2(1), ... z5 405 | p1(1)*p2(3) + p1(2)*p2(2) + p1(3)*p2(1), ... z4 406 | p1(1)*p2(4) + p1(2)*p2(3) + p1(3)*p2(2) + p1(4)*p2(1), ... z3 407 | p1(2)*p2(4) + p1(3)*p2(3) + p1(4)*p2(2), ... z2 408 | p1(3)*p2(4) + p1(4)*p2(3), ... z 409 | p1(4)*p2(4)]; % 1 410 | end 411 | 412 | function po = partial_subtrc(p1, p2) 413 | %PARTIAL_SUBTRC Given polinomials p1 and p2 substract them according to the 414 | %following expression: p1 - z*p2 415 | % p1, p2 - are row vectors with the following arrangement 416 | % xz^2 | xz | x | yz^2 | yz | y | z3 | z2 | z | 1 417 | % po - is a row vector with the following arragement 418 | % xz3 | xz2 | xz | x | yz3 | yz2 | yz | y | z4 | z3 | z2 | z | 1 419 | 420 | po = [-p2(1), p1(1) - p2(2), p1(2) - p2(3), p1(3), ... 421 | -p2(4), p1(4) - p2(5), p1(5) - p2(6), p1(6), ... 422 | -p2(7), p1(7) - p2(8), p1(8) - p2(9), p1(9) - p2(10), p1(10)]; 423 | end 424 | 425 | function B = gj_elim_pp(A) 426 | %GJ_ELIM_PP Given Matriz A we perform partial pivoting as per specified in 427 | 428 | [~,U] = lu(A); 429 | 430 | B = zeros(10,20); 431 | B(1:4,:) = U(1:4,:); 432 | 433 | %Back substitution 434 | B(10,:) = U(10,:)/U(10,10); 435 | B(9,:) = (U(9,:) - U(9,10)*B(10,:))/U(9,9); 436 | B(8,:) = (U(8,:) - U(8,9)*B(9,:) - U(8,10)*B(10,:))/U(8,8); 437 | B(7,:) = (U(7,:) - U(7,8)*B(8,:) - U(7,9)*B(9,:) - U(7,10)*B(10,:))/U(7,7); 438 | B(6,:) = (U(6,:) - U(6,7)*B(7,:) - U(6,8)*B(8,:) - U(6,9)*B(9,:) ... 439 | - U(6,10)*B(10,:))/U(6,6); 440 | B(5,:) = (U(5,:) - U(5,6)*B(6,:) - U(5,7)*B(7,:) - U(5,8)*B(8,:) ... 441 | - U(5,9)*B(9,:) - U(5,10)*B(10,:))/U(5,5); 442 | end 443 | 444 | function pout = p1p1(p1, p2) 445 | %P1P1 Given two first order polynomials with the structure [a_x, 446 | %a_y, a_z, a_w] e returns the second order polynomial x,y,z with the 447 | %structure: pout = [a_x2, a_y2, a_z2, a_xy, a_xz, a_yz, a_x, a_y, a_z, a] 448 | 449 | pout = [p1(1)*p2(1), ... %x2 450 | p1(2)*p2(2), ... %y2 451 | p1(3)*p2(3), ... %z2 452 | p1(1)*p2(2) + p1(2)*p2(1), ... %xy 453 | p1(1)*p2(3) + p1(3)*p2(1), ... %xz 454 | p1(2)*p2(3) + p1(3)*p2(2), ... %yz 455 | p1(1)*p2(4) + p1(4)*p2(1), ... %x 456 | p1(2)*p2(4) + p1(4)*p2(2), ... %y 457 | p1(3)*p2(4) + p1(4)*p2(3), ... %z 458 | p1(4)*p2(4)]; %1 459 | end 460 | 461 | function pout = p2p1(p1,p2) 462 | %P2P1 Given two polynomials, p1 of order 2 and p2 of order 1, with 463 | %unknowns x, y and z, return their product. An order 3 polynomial with 464 | %structure shown below 465 | 466 | pout = [p1(1)*p2(1), ... %x3 467 | p1(2)*p2(2), ... %y3 468 | p1(3)*p2(3), ... %z3 469 | p1(1)*p2(2) + p1(4)*p2(1), ... %x2y 470 | p1(2)*p2(1) + p1(4)*p2(2), ... %xy2 471 | p1(1)*p2(3) + p1(5)*p2(1), ... %x2z 472 | p1(3)*p2(1) + p1(5)*p2(3), ... %xz2 473 | p1(2)*p2(3) + p1(6)*p2(2), ... %y2z 474 | p1(3)*p2(2) + p1(6)*p2(3), ... %yz2 475 | p1(4)*p2(3) + p1(5)*p2(2) + p1(6)*p2(1), ... %xyz 476 | p1(1)*p2(4) + p1(7)*p2(1), ... %x2 477 | p1(2)*p2(4) + p1(8)*p2(2), ... %y2 478 | p1(3)*p2(4) + p1(9)*p2(3), ... %z2 479 | p1(4)*p2(4) + p1(7)*p2(2) + p1(8)*p2(1), ... %xy 480 | p1(5)*p2(4) + p1(7)*p2(3) + p1(9)*p2(1), ... %xz 481 | p1(6)*p2(4) + p1(8)*p2(3) + p1(9)*p2(2), ... %yz 482 | p1(7)*p2(4) + p1(10)*p2(1), ... %x 483 | p1(8)*p2(4) + p1(10)*p2(2), ... %y 484 | p1(9)*p2(4) + p1(10)*p2(3), ... %z 485 | p1(10)*p2(4)]; %1 486 | end 487 | 488 | 489 | 490 | --------------------------------------------------------------------------------