├── KRt_from_P.m ├── README.md ├── rodrigues.m ├── rqGivens.m ├── solve_pnp_ap3p.m ├── solve_pnp_dlt.m ├── solve_pnp_epnp.m ├── solve_pnp_p3p.m └── test_pnp.m /KRt_from_P.m: -------------------------------------------------------------------------------- 1 | % Extract K, R, t from Projection Martix 2 | % 3 | % by ftdlyc 4 | % 5 | % Input 6 | % P: [3 x 4] Camera Projection Martix 7 | % 8 | % Output 9 | % K: [3 x 3] Camera Intrinsic Matrix 10 | % R: [3 x 3] Rotation Matrix 11 | % t: [1 x 3] Translation Vector 12 | % 13 | function [K, R, t] = KRt_from_P(P) 14 | %% QR decomposition 15 | [K, R] = rqGivens(P(1:3, 1:3)); 16 | 17 | %% ensure that the diagonal is positive 18 | if K(3, 3) < 0 19 | K = -K; 20 | R = -R; 21 | end 22 | if K(2, 2) < 0 23 | S = [1 0 0 24 | 0 -1 0 25 | 0 0 1]; 26 | K = K * S; 27 | R = S * R; 28 | end 29 | if K(1, 1) < 0 30 | S = [-1 0 0 31 | 0 1 0 32 | 0 0 1]; 33 | K = K * S; 34 | R = S * R; 35 | end 36 | 37 | %% ensure R determinant == 1 38 | t = linsolve(K, P(:, 4)); 39 | 40 | if det(R) < 0 41 | R = -R; 42 | t = -t; 43 | end 44 | 45 | K = K ./ K(3, 3); 46 | 47 | end 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PnP_Matlab 2 | ### Reference 3 | - DLT 4 | - P3P: Gao X S , Hou X R , Tang J , et al. Complete solution classification for the perspective-three-point problem[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2003, 25(8):0-943. 5 | - EPnP: Moreno-Noguer F , Lepetit V , Fua P . Accurate Non-Iterative O(n) Solution to the PnP Problem[C]// IEEE International Conference on Computer Vision. IEEE, 2007. 6 | - AP3P: Ke T, Roumeliotis S. An Efficient Algebraic Solution to the Perspective-Three-Point Problem[J]. 2017. 7 | -------------------------------------------------------------------------------- /rodrigues.m: -------------------------------------------------------------------------------- 1 | function [out,dout]=rodrigues(in) 2 | 3 | % RODRIGUES Transform rotation matrix into rotation vector and viceversa. 4 | % 5 | % Sintax: [OUT]=RODRIGUES(IN) 6 | % If IN is a 3x3 rotation matrix then OUT is the 7 | % corresponding 3x1 rotation vector 8 | % if IN is a rotation 3-vector then OUT is the 9 | % corresponding 3x3 rotation matrix 10 | % 11 | 12 | %% 13 | %% Copyright (c) March 1993 -- Pietro Perona 14 | %% California Institute of Technology 15 | %% 16 | 17 | %% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995. 18 | %% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !! 19 | 20 | %% BUG when norm(om)=pi fixed -- April 6th, 1997; 21 | %% Jean-Yves Bouguet 22 | 23 | %% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003; 24 | %% Jean-Yves Bouguet 25 | 26 | % BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007 27 | 28 | 29 | [m,n] = size(in); 30 | %bigeps = 10e+4*eps; 31 | bigeps = 10e+20*eps; 32 | 33 | if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector 34 | theta = norm(in); 35 | if theta < eps 36 | R = eye(3); 37 | 38 | %if nargout > 1, 39 | 40 | dRdin = [0 0 0; 41 | 0 0 1; 42 | 0 -1 0; 43 | 0 0 -1; 44 | 0 0 0; 45 | 1 0 0; 46 | 0 1 0; 47 | -1 0 0; 48 | 0 0 0]; 49 | 50 | %end; 51 | 52 | else 53 | if n==length(in) in=in'; end; %% make it a column vec. if necess. 54 | 55 | %m3 = [in,theta] 56 | 57 | dm3din = [eye(3);in'/theta]; 58 | 59 | omega = in/theta; 60 | 61 | %m2 = [omega;theta] 62 | 63 | dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; 64 | 65 | alpha = cos(theta); 66 | beta = sin(theta); 67 | gamma = 1-cos(theta); 68 | omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; 69 | A = omega*omega'; 70 | 71 | %m1 = [alpha;beta;gamma;omegav;A]; 72 | 73 | dm1dm2 = zeros(21,4); 74 | dm1dm2(1,4) = -sin(theta); 75 | dm1dm2(2,4) = cos(theta); 76 | dm1dm2(3,4) = sin(theta); 77 | dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; 78 | 0 0 -1 0 0 0 1 0 0; 79 | 0 1 0 -1 0 0 0 0 0]'; 80 | 81 | w1 = omega(1); 82 | w2 = omega(2); 83 | w3 = omega(3); 84 | 85 | dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; 86 | dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; 87 | dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; 88 | 89 | R = eye(3)*alpha + omegav*beta + A*gamma; 90 | 91 | dRdm1 = zeros(9,21); 92 | 93 | dRdm1([1 5 9],1) = ones(3,1); 94 | dRdm1(:,2) = omegav(:); 95 | dRdm1(:,4:12) = beta*eye(9); 96 | dRdm1(:,3) = A(:); 97 | dRdm1(:,13:21) = gamma*eye(9); 98 | 99 | dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; 100 | 101 | 102 | end; 103 | out = R; 104 | dout = dRdin; 105 | 106 | %% it is prob. a rot matr. 107 | elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... 108 | & (abs(det(in)-1) < bigeps)) 109 | R = in; 110 | 111 | % project the rotation matrix to SO(3); 112 | [U,S,V] = svd(R); 113 | R = U*V'; 114 | 115 | tr = (trace(R)-1)/2; 116 | dtrdR = [1 0 0 0 1 0 0 0 1]/2; 117 | theta = real(acos(tr)); 118 | 119 | 120 | if sin(theta) >= 1e-4, 121 | 122 | dthetadtr = -1/sqrt(1-tr^2); 123 | 124 | dthetadR = dthetadtr * dtrdR; 125 | % var1 = [vth;theta]; 126 | vth = 1/(2*sin(theta)); 127 | dvthdtheta = -vth*cos(theta)/sin(theta); 128 | dvar1dtheta = [dvthdtheta;1]; 129 | 130 | dvar1dR = dvar1dtheta * dthetadR; 131 | 132 | 133 | om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; 134 | 135 | dom1dR = [0 0 0 0 0 1 0 -1 0; 136 | 0 0 -1 0 0 0 1 0 0; 137 | 0 1 0 -1 0 0 0 0 0]; 138 | 139 | % var = [om1;vth;theta]; 140 | dvardR = [dom1dR;dvar1dR]; 141 | 142 | % var2 = [om;theta]; 143 | om = vth*om1; 144 | domdvar = [vth*eye(3) om1 zeros(3,1)]; 145 | dthetadvar = [0 0 0 0 1]; 146 | dvar2dvar = [domdvar;dthetadvar]; 147 | 148 | 149 | out = om*theta; 150 | domegadvar2 = [theta*eye(3) om]; 151 | 152 | dout = domegadvar2 * dvar2dvar * dvardR; 153 | 154 | 155 | else 156 | if tr > 0; % case norm(om)=0; 157 | 158 | out = [0 0 0]'; 159 | 160 | dout = [0 0 0 0 0 1/2 0 -1/2 0; 161 | 0 0 -1/2 0 0 0 1/2 0 0; 162 | 0 1/2 0 -1/2 0 0 0 0 0]; 163 | else 164 | 165 | % case norm(om)=pi; 166 | if(0) 167 | 168 | %% fixed April 6th by Bouguet -- not working in all cases! 169 | out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); 170 | %keyboard; 171 | 172 | else 173 | 174 | % Solution by Mike Burl on Feb 27, 2007 175 | % This is a better way to determine the signs of the 176 | % entries of the rotation vector using a hash table on all 177 | % the combinations of signs of a pairs of products (in the 178 | % rotation matrix) 179 | 180 | % Define hashvec and Smat 181 | hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11]; 182 | Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1; 183 | 1,-1,-1; 1,-1,1]; 184 | 185 | M = (R+eye(3,3))/2; 186 | uabs = sqrt(M(1,1)); 187 | vabs = sqrt(M(2,2)); 188 | wabs = sqrt(M(3,3)); 189 | 190 | mvec = ([M(1,2), M(2,3), M(1,3)] + [M(2,1), M(3,2), M(3,1)])/2; 191 | syn = ((mvec > eps) - (mvec < -eps)); % robust sign() function 192 | hash = syn * [9; 3; 1]; 193 | idx = find(hash == hashvec); 194 | svec = Smat(idx,:)'; 195 | 196 | out = theta * [uabs; vabs; wabs] .* svec; 197 | 198 | end; 199 | 200 | if nargout > 1, 201 | fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); 202 | dout = NaN*ones(3,9); 203 | end; 204 | end; 205 | end; 206 | 207 | else 208 | error('Neither a rotation matrix nor a rotation vector were provided'); 209 | end; 210 | 211 | return; 212 | 213 | %% test of the Jacobians: 214 | 215 | %% TEST OF dRdom: 216 | om = randn(3,1); 217 | dom = randn(3,1)/1000000; 218 | [R1,dR1] = rodrigues(om); 219 | R2 = rodrigues(om+dom); 220 | R2a = R1 + reshape(dR1 * dom,3,3); 221 | gain = norm(R2 - R1)/norm(R2 - R2a) 222 | 223 | %% TEST OF dOmdR: 224 | om = randn(3,1); 225 | R = rodrigues(om); 226 | dom = randn(3,1)/10000; 227 | dR = rodrigues(om+dom) - R; 228 | 229 | [omc,domdR] = rodrigues(R); 230 | [om2] = rodrigues(R+dR); 231 | om_app = omc + domdR*dR(:); 232 | gain = norm(om2 - omc)/norm(om2 - om_app) 233 | 234 | 235 | %% OTHER BUG: (FIXED NOW!!!) 236 | omu = randn(3,1); 237 | omu = omu/norm(omu) 238 | om = pi*omu; 239 | [R,dR]= rodrigues(om); 240 | [om2] = rodrigues(R); 241 | [om om2] 242 | 243 | %% NORMAL OPERATION 244 | om = randn(3,1); 245 | [R,dR]= rodrigues(om); 246 | [om2] = rodrigues(R); 247 | [om om2] 248 | 249 | %% Test: norm(om) = pi 250 | u = randn(3,1); 251 | u = u / sqrt(sum(u.^2)); 252 | om = pi*u; 253 | R = rodrigues(om); 254 | R2 = rodrigues(rodrigues(R)); 255 | norm(R - R2) 256 | 257 | %% Another test case where norm(om)=pi from Chen Feng (June 27th, 2014) 258 | R = [-0.950146567583153 -6.41765854280073e-05 0.311803617668748; ... 259 | -6.41765854277654e-05 -0.999999917385145 -0.000401386434914383; ... 260 | 0.311803617668748 -0.000401386434914345 0.950146484968298]; 261 | om = rodrigues(R) 262 | norm(om) - pi 263 | 264 | %% Another test case where norm(om)=pi from 余成义 (July 1st, 2014) 265 | R = [-0.999920129411407 -6.68593208347372e-05 -0.0126384464118876; ... 266 | 9.53007036072085e-05 -0.999997464662094 -0.00224979713751896; ... 267 | -0.0126382639492467 -0.00225082189773293 0.999917600647740]; 268 | om = rodrigues(R) 269 | norm(om) - pi 270 | 271 | 272 | 273 | -------------------------------------------------------------------------------- /rqGivens.m: -------------------------------------------------------------------------------- 1 | function [ R, Q ] = rqGivens( A ) 2 | % rqGivens Calculates RQ decomposition of A = RQ (3x3) 3 | % 4 | % Syntax: 5 | % [R, Q] = rqGivens(A); 6 | % 7 | % Input: 8 | % A - 3-by-3 matrix of rank 3 9 | % 10 | % Output: 11 | % R - Upper triangular matrix (3-by-3) 12 | % Q - Orthogonal matrix (3-by-3) 13 | % 14 | % Description: 15 | % This function calculates the 3-dimensional RQ decomposition of A using 16 | % Givens rotations (equal to Euler rotations) Gx, Gy Gz: 17 | % 18 | % Gx = [ 1 0 0; 19 | % 0 c -s; 20 | % 0 s c]; 21 | % 22 | % Gy = [ c 0 s; 23 | % 0 1 0; 24 | % -s 0 c]; 25 | % 26 | % Gz = [ c -s 0; 27 | % s c 0; 28 | % 0 0 1]; 29 | % 30 | % Ax = A * Gx to set Ax(3,2) to zero. 31 | % Axy = Ax * Gy to set Axy(3,1) to zero. 32 | % R = Axyz = Axy * Gz to set Axyz(2,1) to zero. 33 | % 34 | % R = A * Gx * Gy * Gz 35 | % -> R * Gz' * Gy' * Gx' = A 36 | % -> Q = Gz' * Gy' * Gx' 37 | % 38 | % See also: 39 | % - https://en.wikipedia.org/wiki/Givens_rotation#Dimension_3 40 | % - Hartley, Zisserman - Multiple View Geometry in Computer Vision 41 | % http://www.amazon.com/dp/0521540518 (Appendix 4, A4.1.1, page 579) 42 | % 43 | % Author: Lars Meinel 44 | % Email: lars.meinel@etit.tu-chemnitz.de 45 | % Date: July 2015; Last revision: 2015-07-10 46 | %% 1st - Set element 32 to zero 47 | if A(3,2) == 0 48 | Ax = A; 49 | Gx = eye(3); 50 | else 51 | r32 = sqrt(A(3,3)*A(3,3) + A(3,2)*A(3,2)); 52 | c32 = A(3,3) / r32; 53 | s32 = -A(3,2) / r32; 54 | G32 = [ 1 0 0; 55 | 0 c32 -s32; 56 | 0 s32 c32 ]; 57 | Gx = G32; 58 | Ax = A * Gx; 59 | end 60 | %% 2nd - Set element 31 to zero 61 | if A(3,1) == 0 62 | Axy = Ax; 63 | Gy = eye(3); 64 | else 65 | r31 = sqrt(Ax(3,3)*Ax(3,3) + Ax(3,1)*Ax(3,1)); 66 | c31 = Ax(3,3) / r31; 67 | s31 = Ax(3,1) / r31; 68 | G31 = [ c31 0 s31; 69 | 0 1 0; 70 | -s31 0 c31]; 71 | Gy = G31; 72 | Axy = Ax * Gy; 73 | end 74 | %% 3rd - Set element 21 to zero 75 | if A(2,1) == 0 76 | Axyz = Axy; 77 | Gz = eye(3); 78 | else 79 | r21 = sqrt(Axy(2,2)*Axy(2,2) + Axy(2,1)*Axy(2,1)); 80 | c21 = Axy(2,2) / r21; 81 | s21 = -Axy(2,1) / r21; 82 | G21 = [ c21 -s21 0; 83 | s21 c21 0; 84 | 0 0 1]; 85 | Gz = G21; 86 | Axyz = Axy * Gz; 87 | end 88 | R = Axyz; 89 | Q = Gz' * Gy' * Gx'; 90 | end -------------------------------------------------------------------------------- /solve_pnp_ap3p.m: -------------------------------------------------------------------------------- 1 | % Perspective-n-Point 2 | % AP3P Method 3 | % Reference: An Efficient Algebraic Solution to the Perspective-Three-Point Problem 4 | % 5 | % by ftdlyc 6 | % 7 | % Input 8 | % X: [3 x n] or [4 x n] 3D points 9 | % x: [2 x n] or [3 x n] 2D points 10 | % 11 | % Output 12 | % P: n x [3 x 4] cell of Camera Projection Martix 13 | % 14 | function P = solve_pnp_p3p(X, x) 15 | P = cell(0, 0); 16 | 17 | [row_X, col_X] = size(X); 18 | [row_x, col_x] = size(x); 19 | if row_X ~= 3 && row_X ~= 4 20 | fprintf('mat X must be [3 x n] or [4 x n]\n') 21 | return 22 | end 23 | if row_x ~= 2 && row_x ~= 3 24 | fprintf('mat x must be [2 x n] or [3 x n]\n') 25 | return 26 | end 27 | if col_X ~= col_x 28 | fprintf('col(x) no equal to col(X)\n') 29 | return 30 | end 31 | n = col_X; 32 | if n ~= 3 33 | fprintf('col(x)/col(X) must be 3\n') 34 | return 35 | end 36 | 37 | if row_X == 4 38 | X = X(1:3, :) ./ X(4, :); 39 | end 40 | if row_x == 2 41 | x(3, :) = ones(1, col_x); 42 | end 43 | 44 | p1 = X(:, 1); 45 | p2 = X(:, 2); 46 | p3 = X(:, 3); 47 | b1 = x(:, 1); 48 | b2 = x(:, 2); 49 | b3 = x(:, 3); 50 | 51 | %% compute k1, k3 52 | k1 = (p1 - p2) / norm(p1 - p2); 53 | k3 = cross(b1, b2); 54 | nk3 = norm(k3); 55 | k3 = k3 / nk3; 56 | 57 | %% compute u1, u2, v1, v2 58 | u1 = p1 - p3; 59 | u2 = p2 - p3; 60 | v1 = cross(b1, b3); 61 | v2 = cross(b2, b3); 62 | 63 | %% compute delta, nl 64 | nl = cross(u1, k1); 65 | delta = norm(nl); 66 | nl = nl / delta; 67 | 68 | %% compute f 69 | k3b3 = k3' * b3; 70 | b1b2 = b1' * b2; 71 | 72 | f11 = delta * k3b3; 73 | f21 = delta * b1b2 * k3b3; 74 | f22 = delta * k3b3 * nk3; 75 | f13 = delta * v1' * k3; 76 | f23 = delta * v2' * k3; 77 | f24 = u2' * k1 * k3b3 * nk3; 78 | f15 = - u1' * k1 * k3b3; 79 | f25 = - u2' * k1 * b1b2 * k3b3; 80 | 81 | %% compute quartic polynomial coeffs 82 | g1 = f13 * f22; 83 | g2 = f13 * f25 - f15 * f23; 84 | g3 = f11 * f23 - f13 * f21; 85 | g4 = -f13 * f24; 86 | g5 = f11 * f22; 87 | g6 = f11 * f25 - f15 * f21; 88 | g7 = -f15 * f24; 89 | 90 | coeff4 = g5 * g5 + g1 * g1 + g3 * g3; 91 | coeff3 = 2 * (g5 * g6 + g1 * g2 + g3 * g4); 92 | coeff2 = g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3; 93 | coeff1 = 2 * (g6 * g7 - g1 * g2 - g3 * g4); 94 | coeff0 = g7 * g7 - g2 * g2 - g4 * g4; 95 | 96 | %% solve quartic polynomial and refine 97 | s = roots([coeff4, coeff3, coeff2, coeff1, coeff0]); 98 | real_roots = zeros(4, 1); 99 | 100 | niters = 2; 101 | for i = 1:4 102 | root = real(s(i)); 103 | for j = 1:2 104 | error = coeff0 + root * (coeff1 + root * (coeff2 + root * (coeff3 + root * coeff4))); 105 | derivative = coeff1 + root * (2 * coeff2 + root * (3 * coeff3 + 4 * root * coeff4)); 106 | root = root - error / derivative; 107 | end 108 | real_roots(i) = root; 109 | end 110 | 111 | %% compute C and pc 112 | Ck1nl = [k1 nl cross(k1, nl)]; 113 | Cb1k3T = [b1 k3 cross(b1, k3)]'; 114 | 115 | n = 1; 116 | for i = 1:4 117 | ctheta1 = real_roots(i); 118 | if abs(ctheta1) > 1 119 | continue 120 | end 121 | 122 | stheta1 = sign(k3b3) * sqrt(1 - ctheta1 * ctheta1); 123 | ntheta3 = stheta1 / (g5 * ctheta1 * ctheta1 + g6 * ctheta1 + g7); 124 | ctheta3 = ntheta3 * (g1 * ctheta1 + g2); 125 | stheta3 = ntheta3 * (g3 * ctheta1 + g4); 126 | 127 | C = Ck1nl ... 128 | * [1 0 0 ; ... 129 | 0 ctheta1 stheta1; ... 130 | 0 -stheta1 ctheta1] ... 131 | * [ctheta3 0 -stheta3; ... 132 | 0 1 0 ; ... 133 | stheta3 0 ctheta3] ... 134 | * Cb1k3T; 135 | 136 | R = C'; 137 | t = delta * stheta1 / k3b3 * b3 - R * p3; 138 | P{n} = [R, t]; 139 | n = n + 1; 140 | end 141 | end 142 | -------------------------------------------------------------------------------- /solve_pnp_dlt.m: -------------------------------------------------------------------------------- 1 | % Perspective-n-Point 2 | % Direct Linear Transform 3 | % 4 | % by ftdlyc 5 | % 6 | % Input 7 | % X: [3 x n] or [4 x n] 3D points 8 | % x: [2 x n] or [3 x n] 2D points 9 | % 10 | % Output 11 | % P: [3 x 4] Camera Projection Martix 12 | % 13 | function P = solve_pnp_dlt(X, x) 14 | [row_X, col_X] = size(X); 15 | [row_x, col_x] = size(x); 16 | if row_X ~= 3 && row_X ~= 4 17 | fprintf('mat X must be [3 x n] or [4 x n]\n') 18 | return 19 | end 20 | if row_x ~= 2 && row_x ~= 3 21 | fprintf('mat x must be [2 x n] or [3 x n]\n') 22 | return 23 | end 24 | if col_X ~= col_x 25 | fprintf('col(x) no equal to col(X)\n') 26 | return 27 | end 28 | n = col_X; 29 | if n < 6 30 | fprintf('col(x)/col(X) must greather than 6\n') 31 | return 32 | end 33 | 34 | if row_X == 3 35 | X(4, :) = ones(1, col_X); 36 | end 37 | 38 | %% build action matrix 39 | A = zeros(n * 2, 12); 40 | for i = 1:n 41 | pt3d = X(:, i)'; 42 | A(2 * i - 1, :) = [pt3d, zeros(1, 4), -x(1, i) .* pt3d]; 43 | A(2 * i, :) = [zeros(1, 4), pt3d, -x(2, i) .* pt3d]; 44 | end 45 | 46 | %% Solve P 47 | [~, D, V] = svd(A); 48 | radio = D(11, 11) / D(1, 1); 49 | if radio > 1e-5 50 | P = V(:, 12); 51 | P = reshape(P, 4, 3)'; 52 | P = P ./ P(3, 4); 53 | else 54 | P = [eye(3) zeros(3, 1)]; 55 | end 56 | 57 | end 58 | -------------------------------------------------------------------------------- /solve_pnp_epnp.m: -------------------------------------------------------------------------------- 1 | % Perspective-n-Point 2 | % EPnP Method 3 | % Reference: EPnP An Accurate O(n) Solution to the PnP Problem 4 | % 5 | % by ftdlyc 6 | % 7 | % Input 8 | % X: [3 x n] or [4 x n] 3D points 9 | % x: [2 x n] or [3 x n] 2D points 10 | % 11 | % Output 12 | % P: [3 x 4] cell of Camera Projection Martix 13 | % 14 | function P = solve_pnp_epnp(X, x) 15 | err = 1e10; 16 | R = eye(3); 17 | t = zeros(3, 1); 18 | P = [R t]; 19 | 20 | [row_X, col_X] = size(X); 21 | [row_x, col_x] = size(x); 22 | if row_X ~= 3 && row_X ~= 4 23 | fprintf('mat X must be [3 x n] or [4 x n]\n') 24 | return 25 | end 26 | if row_x ~= 2 && row_x ~= 3 27 | fprintf('mat x must be [2 x n] or [3 x n]\n') 28 | return 29 | end 30 | if col_X ~= col_x 31 | fprintf('col(x) no equal to col(X)\n') 32 | return 33 | end 34 | n = col_X; 35 | if n <= 3 36 | fprintf('col(x)/col(X) must greather than 3\n') 37 | return 38 | end 39 | 40 | if row_X == 4 41 | X = X(1:3, :) ./ X(4, :); 42 | end 43 | if row_x == 3 44 | x = x(1:2, :) ./ x(3, :); 45 | end 46 | 47 | %% choose control point 48 | C = zeros(3, 4); 49 | C(:, 1) = sum(X, 2) ./ n; 50 | A = X - C(:, 1); 51 | [v, lambda] = eig(A * A'); 52 | for i = 1:3 53 | C(:, i + 1) = C(:, 1) + sqrt(lambda(i, i) / n) * v(:, i); 54 | end 55 | 56 | %% compute barycentric coordinates 57 | % 4 x n matrix 58 | % [X; 1] = [C; 1] * alpha 59 | alpha = pinv([C; ones(1, 4)]) * [X; ones(1, n)]; 60 | 61 | %% compute V 62 | fx = 1; 63 | fy = 1; 64 | cx = 0; 65 | cy = 0; 66 | M = zeros(2 * n, 12); 67 | for i = 1:n 68 | M(2 * i - 1, :) = [alpha(1, i) * fx, 0, alpha(1, i) * (cx - x(1, i)) ... 69 | alpha(2, i) * fx, 0, alpha(2, i) * (cx - x(1, i)) ... 70 | alpha(3, i) * fx, 0, alpha(3, i) * (cx - x(1, i)) ... 71 | alpha(4, i) * fx, 0, alpha(4, i) * (cx - x(1, i))]; 72 | M(2 * i, :) = [0, alpha(1, i) * fy, alpha(1, i) * (cy - x(2, i)) ... 73 | 0, alpha(2, i) * fy, alpha(2, i) * (cy - x(2, i)) ... 74 | 0, alpha(3, i) * fy, alpha(3, i) * (cy - x(2, i)) ... 75 | 0, alpha(4, i) * fy, alpha(4, i) * (cy - x(2, i))]; 76 | end 77 | [~, ~, V] = svd(M); 78 | 79 | %% compute L_6x10 and rho 80 | % betas10 = [b11 b12 b22 b13 b23 b33 b14 b24 b34 b44] 81 | % L = [l12'; l13'; l14'; l23'; l24'; l34';] 82 | v = zeros(12, 4); 83 | v(:, 1) = V(:, 12); 84 | v(:, 2) = V(:, 11); 85 | v(:, 3) = V(:, 10); 86 | v(:, 4) = V(: , 9); 87 | 88 | s = cell(4, 6); 89 | for i = 1:4 90 | vtmp1 = v(1:3, i); 91 | vtmp2 = v(4:6, i); 92 | vtmp3 = v(7:9, i); 93 | vtmp4 = v(10:12, i); 94 | s{i, 1} = vtmp1 - vtmp2; 95 | s{i, 2} = vtmp1 - vtmp3; 96 | s{i, 3} = vtmp1 - vtmp4; 97 | s{i, 4} = vtmp2 - vtmp3; 98 | s{i, 5} = vtmp2 - vtmp4; 99 | s{i, 6} = vtmp3 - vtmp4; 100 | end 101 | 102 | L = zeros(6, 10); 103 | for i = 1:6 104 | L(i, :) = [ s{1, i}' * s{1, i} ... 105 | 2 * s{1, i}' * s{2, i} ... 106 | s{2, i}' * s{2, i} ... 107 | 2 * s{1, i}' * s{3, i} ... 108 | 2 * s{2, i}' * s{3, i} ... 109 | s{3, i}' * s{3, i} ... 110 | 2 * s{1, i}' * s{4, i} ... 111 | 2 * s{2, i}' * s{4, i} ... 112 | 2 * s{3, i}' * s{4, i} ... 113 | s{4, i}' * s{4, i}]; 114 | end 115 | 116 | rho = zeros(6 , 1); 117 | rho(1) = sum((C(:, 1) - C(:, 2)).^2); 118 | rho(2) = sum((C(:, 1) - C(:, 3)).^2); 119 | rho(3) = sum((C(:, 1) - C(:, 4)).^2); 120 | rho(4) = sum((C(:, 2) - C(:, 3)).^2); 121 | rho(5) = sum((C(:, 2) - C(:, 4)).^2); 122 | rho(6) = sum((C(:, 3) - C(:, 4)).^2); 123 | 124 | %% compute beta (N = 1) 125 | % find betas 126 | betas = zeros(4, 1); 127 | b1 = linsolve(L(:, 1), rho); 128 | betas(1) = sqrt(abs(b1)); 129 | 130 | betas = gauss_newton(L, rho, betas); 131 | 132 | Xc = compute_Xc(alpha, betas, v); 133 | [R1, t1] = compute_Rt(X, Xc); 134 | err1 = reproject_error(X, x, R1, t1); 135 | 136 | if err1 < err 137 | err = err1; 138 | R = R1; 139 | t = t1; 140 | end 141 | 142 | %% compute beta (N = 2) 143 | % betas10 = [b11 b12 b22 b13 b23 b33 b14 b24 b34 b44] 144 | % b3 = [b11 b12 b22] 145 | % find beta 146 | betas = zeros(4, 1); 147 | b3 = linsolve(L(:, 1:3), rho); 148 | betas(1) = sqrt(abs(b3(1))); 149 | betas(2) = sqrt(abs(b3(3))) * sign(b3(2)) * sign(b3(1)); 150 | 151 | betas = gauss_newton(L, rho, betas); 152 | 153 | Xc = compute_Xc(alpha, betas, v); 154 | [R2, t2] = compute_Rt(X, Xc); 155 | 156 | err2 = reproject_error(X, x, R2, t2); 157 | if err2 < err 158 | err = err2; 159 | R = R2; 160 | t = t2; 161 | end 162 | 163 | %% compute beta (N = 3) 164 | % betas10 = [b11 b12 b22 b13 b23 b33 b14 b24 b34 b44] 165 | % b6 = [b11 b12 b22 b13 b23 b33] 166 | % find beta 167 | betas = zeros(4, 1); 168 | b6 = linsolve(L(:, 1:6), rho); 169 | betas(1) = sqrt(abs(b6(1))); 170 | betas(2) = sqrt(abs(b6(3))) * sign(b6(2)) * sign(b6(1)); 171 | betas(3) = sqrt(abs(b6(6))) * sign(b6(4)) * sign(b6(1)); 172 | 173 | betas = gauss_newton(L, rho, betas); 174 | 175 | Xc = compute_Xc(alpha, betas, v); 176 | [R3, t3] = compute_Rt(X, Xc); 177 | 178 | err3 = reproject_error(X, x, R3, t3); 179 | if err3 < err 180 | err = err3; 181 | R = R3; 182 | t = t3; 183 | end 184 | 185 | %% compute beta (N = 4) 186 | % betas10 = [b11 b12 b22 b13 b23 b33 b14 b24 b34 b44] 187 | % b4 = [b11 b12 b13 b14] 188 | % find beta 189 | betas = zeros(4, 1); 190 | b4 = linsolve([L(:, 1), L(:, 2), L(:, 4), L(:, 7)], rho); 191 | betas(1) = sqrt(abs(b4(1))); 192 | betas(2) = b4(2) / betas(1); 193 | betas(3) = b4(3) / betas(1); 194 | betas(4) = b4(4) / betas(1); 195 | 196 | betas = gauss_newton(L, rho, betas); 197 | 198 | Xc = compute_Xc(alpha, betas, v); 199 | [R4, t4] = compute_Rt(X, Xc); 200 | 201 | err4 = reproject_error(X, x, R4, t4); 202 | if err4 < err 203 | err = err4; 204 | R = R4; 205 | t = t4; 206 | end 207 | 208 | %% return 209 | P = [R t]; 210 | P = P ./ P(3, 4); 211 | end 212 | 213 | function betas = gauss_newton(L, rho, betas) 214 | n_iter = 5; 215 | for i = 1:n_iter 216 | % 6 x 10 217 | % err 12, 13, 14, 23, 24, 34 218 | % betas 11 12 22 13 23 33 14 24 34 44 219 | J = zeros(6, 4); 220 | for j = 1:6 221 | J(j, 1) = 2 * L(j, 1) * betas(1) + L(j, 2) * betas(2) + L(j, 4) * betas(3) + L(j, 7) * betas(4); 222 | J(j, 2) = L(j, 2) * betas(1) + 2 * L(j, 3) * betas(2) + L(j, 5) * betas(3) + L(j, 8) * betas(4); 223 | J(j, 3) = L(j, 4) * betas(1) + L(j, 5) * betas(2) + 2 * L(j, 6) * betas(3) + L(j, 9) * betas(4); 224 | J(j, 4) = L(j, 7) * betas(1) + L(j, 8) * betas(2) + L(j, 9) * betas(3) + 2 * L(j, 10) * betas(4); 225 | end 226 | 227 | % 6 x 1 228 | % err 12, 13, 14, 23, 24, 34 229 | r = zeros(6, 1); 230 | for j = 1:6 231 | r(j) = rho(j) ... 232 | - L(j, 1) * betas(1) * betas(1) ... 233 | - L(j, 2) * betas(1) * betas(2) ... 234 | - L(j, 3) * betas(2) * betas(2) ... 235 | - L(j, 4) * betas(1) * betas(3) ... 236 | - L(j, 5) * betas(2) * betas(3) ... 237 | - L(j, 6) * betas(3) * betas(3) ... 238 | - L(j, 7) * betas(1) * betas(4) ... 239 | - L(j, 8) * betas(2) * betas(4) ... 240 | - L(j, 9) * betas(3) * betas(4) ... 241 | - L(j, 10) * betas(4) * betas(4); 242 | end 243 | 244 | A = J' * J; 245 | b = J' * r; 246 | dbetas = linsolve(A, b); 247 | 248 | betas = betas + dbetas; 249 | end 250 | end 251 | 252 | function Xc = compute_Xc(alpha, betas, v) 253 | x = betas(1) * v(:, 1) + betas(2) * v(:, 2) + betas(3) * v(:, 3) + betas(4) * v(:, 4); 254 | C = [x(1:3, 1), x(4:6, 1), x(7:9, 1), x(10:12, 1)]; 255 | Xc = C * alpha; 256 | end 257 | 258 | function [R, t] = compute_Rt(Xw, Xc) 259 | mean_Xw = mean(Xw, 2); 260 | mean_Xc = mean(Xc, 2); 261 | 262 | new_Xw = Xw - mean_Xw; 263 | new_Xc = Xc - mean_Xc; 264 | 265 | W = zeros(3, 3); 266 | for j = 1:size(Xw, 2) 267 | W = W + new_Xc(:, j) * new_Xw(:, j)'; 268 | end 269 | 270 | [U, ~, V] = svd(W); 271 | R = U * V'; 272 | t = mean_Xc - R * mean_Xw; 273 | end 274 | 275 | function err = reproject_error(X, x, R, t) 276 | x2 = R * X + t; 277 | for i = 1:size(X, 2) 278 | x2(:, i) = x2(:, i) ./ x2(3, i); 279 | end 280 | x2 = x2(1:2, :); 281 | err = mean(sqrt(sum((x2 - x).^2, 1))); 282 | end 283 | -------------------------------------------------------------------------------- /solve_pnp_p3p.m: -------------------------------------------------------------------------------- 1 | % Perspective-n-Point 2 | % P3P Method 3 | % Reference: Complete Solution Classification for the Perspective-Three-Point Problem 4 | % 5 | % by ftdlyc 6 | % 7 | % Input 8 | % X: [3 x n] or [4 x n] 3D points 9 | % x: [2 x n] or [3 x n] 2D points 10 | % 11 | % Output 12 | % P: n x [3 x 4] cell of Camera Projection Martix 13 | % 14 | function P = solve_pnp_p3p(X, x) 15 | P = cell(0, 0); 16 | 17 | [row_X, col_X] = size(X); 18 | [row_x, col_x] = size(x); 19 | if row_X ~= 3 && row_X ~= 4 20 | fprintf('mat X must be [3 x n] or [4 x n]\n') 21 | return 22 | end 23 | if row_x ~= 2 && row_x ~= 3 24 | fprintf('mat x must be [2 x n] or [3 x n]\n') 25 | return 26 | end 27 | if col_X ~= col_x 28 | fprintf('col(x) no equal to col(X)\n') 29 | return 30 | end 31 | n = col_X; 32 | if n ~= 3 33 | fprintf('col(x)/col(X) must be 3\n') 34 | return 35 | end 36 | 37 | if row_X == 4 38 | X = X(1:3, :) ./ X(4, :); 39 | end 40 | if row_x == 2 41 | x(3, :) = ones(1, col_x); 42 | end 43 | 44 | % verification that world points are not colinear 45 | if norm(cross(X(:, 2) - X(:, 1), X(:, 3) - X(:, 1))) < 1e-5 46 | fprintf('point A, B, C are colinear\n') 47 | return; 48 | end 49 | 50 | %% cal point (A, B, C) in camera coordinate 51 | x_norm = x ./ sqrt(sum(x.^2, 1)); 52 | cos_bc = x_norm(:, 2)' * x_norm(:, 3); 53 | cos_ac = x_norm(:, 1)' * x_norm(:, 3); 54 | cos_ab = x_norm(:, 1)' * x_norm(:, 2); 55 | 56 | dis_bc = norm(X(:, 2) - X(:, 3)); 57 | dis_ac = norm(X(:, 1) - X(:, 3)); 58 | dis_ab = norm(X(:, 1) - X(:, 2)); 59 | 60 | lenghts = solve_for_lengths([cos_bc, cos_ac, cos_ab]', [dis_bc, dis_ac, dis_ab]'); 61 | [~, n] = size(lenghts); 62 | 63 | %% solve 3D-3D problem 64 | for i = 1:n 65 | pts = x_norm .* lenghts(:, i)'; 66 | 67 | mean_pts = mean(pts, 2); 68 | mean_X = mean(X, 2); 69 | 70 | new_pts= pts - mean_pts; 71 | new_X = X - mean_X; 72 | 73 | W = zeros(3, 3); 74 | for j = 1:3 75 | W = W + new_pts(:, j) * new_X(:, j)'; 76 | end 77 | 78 | [U, ~, V] = svd(W); 79 | R = U * V'; 80 | t = mean_pts - R * mean_X; 81 | 82 | P{i} = [R, t]; 83 | end 84 | 85 | end 86 | 87 | % Given 3D distances between three points and cosines of 3 angles at the apex 88 | % calculates the lentghs of the line segments connecting projection center 0 and 89 | % the three 3D points (A, B, C). 90 | % 91 | % by ftdlyc 92 | % 93 | % Input 94 | % cosines: [cos, cos, cos]' 95 | % distances: [|BC|, |AC|, |AB|]' 96 | % 97 | % Output 98 | % lengths: [3 x n] 99 | % [|OA| ... 100 | % |OB| ... 101 | % |OC| ...] 102 | % 103 | function lengths = solve_for_lengths(cosines, distances) 104 | n = 0; 105 | lengths = []; 106 | 107 | p = 2 * cosines(1); 108 | q = 2 * cosines(2); 109 | r = 2 * cosines(3); 110 | a = distances(1) * distances(1) / (distances(3) * distances(3)); 111 | b = distances(2) * distances(2) / (distances(3) * distances(3)); 112 | 113 | a2 = a * a; 114 | b2 = b * b; 115 | q2 = q * q; 116 | p2 = p * p; 117 | r2 = r * r; 118 | 119 | coeff4 = a2 + b2 - 2 * a - 2 * b + (2 - r2) * b * a + 1; 120 | coeff3 = -2 * q * a2 - r * p * b2 + 4 * q * a + (2 * q + p * r) * b ... 121 | + (r2 * q - 2 * q + r * p) * a * b - 2 * q; 122 | coeff2 = (2 + q2) * a2 + (p2 + r2 - 2) * b2 - (4 + 2 * q2) * a ... 123 | - (p * q * r + p2) * b - (p * q * r + r2) * a * b + q2 + 2; 124 | coeff1 = -2 * q * a2 - r * p * b2 + 4 * q * a + (p * r + q * p2 - 2 * q) * b ... 125 | + (r * p + 2 * q) * a * b - 2 * q; 126 | coeff0 = a2 + b2 - 2 * a + (2 - p2) * b - 2 * a * b + 1; 127 | b1 = b * ((p2 - p * q * r + r2) * a + (p2 - r2) * b - p2 + p * q * r - r2)^2; 128 | 129 | if b1 == 0 || coeff4 == 0 130 | return 131 | end 132 | 133 | x_roots = roots([coeff4, coeff3, coeff2, coeff1, coeff0]); 134 | 135 | for i = 1:size(x_roots, 1) 136 | x = real(x_roots(i)); 137 | if x <= 0 138 | continue 139 | end 140 | 141 | b0 = ((1 - a - b) * x^2 + (a - 1) * q * x - a + b + 1) * ( ... 142 | r^3 * (a2 + b2 - 2 * a - 2 * b + (2 - r2) * a * b + 1) * x^3 ... 143 | + r^2 * (p + p * a2 - 2 * r * q * a * b + 2 * r * q * b - 2 * r * q ... 144 | -2 * p * a - 2 * p * b + p * r2 * b + 4 * r * q * a ... 145 | + q * r^3 * a * b - 2 * r * q * a2 + 2 * p * a * b + p * b2 ... 146 | - r2 * p * b2) * x^2 ... 147 | + (r^5 * (b2 - a * b) ... 148 | - r^4 * p * q * b ... 149 | + r^3 * (q2 - 4 * a - 2 * q2 * a + q2 * a2 + 2 * a2 - 2 * b2 + 2) ... 150 | + r^2 * (4 * p * q * a - 2 * p * q * a * b + 2 * p * q * b ... 151 | - 2 * p * q - 2 * p * q * a2) ... 152 | + r * (p2 * b2 - 2 * p2 * b + 2 * p2 * a * b - 2 * p2 * a ... 153 | + p2 + p2 * a2)) * x ... 154 | + (2 * p * r2 - 2 * r^3 * q + p^3 - 2 * p2 * q * r + p * q2 * r2) * a2 ... 155 | + (p^3 - 2 * p * r2) * b2 ... 156 | + (4 * q * r^3 - 4 * p * r2 - 2 * p^3 + 4 * p2 * q * r - 2 * p * q2 * r2) * a ... 157 | + (-2 * q * r^3 + p * r^4 + 2 * p2 * q * r- 2 * p^3) * b ... 158 | + (2 * p^3 + 2 * q * r^3 - 2 * p2 * q * r) * a * b ... 159 | + p * q2 * r2 - 2 * p2 * q * r + 2 * p * r2 + p^3 - 2 * r^3 * q ... 160 | ); 161 | 162 | if b0 <= 0 163 | continue 164 | end 165 | 166 | y = b0 / b1; 167 | u = x * x + y * y - x * y * r; 168 | 169 | if u <= 0 170 | continue 171 | end 172 | 173 | n = n + 1; 174 | Z = distances(3) / sqrt(u); 175 | X = x * Z; 176 | Y = y * Z; 177 | 178 | lengths(1, n) = X; 179 | lengths(2, n) = Y; 180 | lengths(3, n) = Z; 181 | end 182 | 183 | end 184 | -------------------------------------------------------------------------------- /test_pnp.m: -------------------------------------------------------------------------------- 1 | %% make date 2 | clc 3 | clear 4 | 5 | fprintf('---------------------------PNP test---------------------------\n\n') 6 | 7 | A = rand(3, 3); 8 | t = rand(3, 1); 9 | [R, ~] = qr(A); 10 | P = [R, t]; 11 | 12 | npoints = 6; 13 | X = rand(3, npoints); 14 | X(4, :) = ones(1, npoints); 15 | 16 | x = P * X; 17 | for i = 1:npoints 18 | x(:, i) = x(:, i) ./ x(3, i); 19 | end 20 | 21 | errors = zeros(npoints, 4); 22 | 23 | fprintf('number of points = %d\n', npoints) 24 | 25 | %% add noise 26 | std = 0.005; 27 | noise = normrnd(0, std, 3, npoints); 28 | X_noise = X; 29 | X_noise(1:3, :) = X_noise(1:3, :) + noise; 30 | 31 | P_norm = P ./ P(3, 4); 32 | 33 | fprintf('noise std = %f\n\n', std) 34 | 35 | %% PNP DTL 36 | fprintf('---------------------------PNP DLT---------------------------\n\n') 37 | 38 | P_est = solve_pnp_dlt(X_noise, x); 39 | [K_est, R_est, t_est] = KRt_from_P(P_est); 40 | 41 | angle_axis = rodrigues(R); 42 | angle_axis_est = rodrigues(R_est); 43 | 44 | fprintf('angle_axis = \n') 45 | disp(angle_axis') 46 | fprintf('angle_axis_est = \n') 47 | disp(angle_axis_est') 48 | fprintf('t = \n') 49 | disp(t') 50 | fprintf('t_est = \n') 51 | disp(t_est') 52 | 53 | x_est = [R_est, t_est] * X; 54 | for i = 1:npoints 55 | x_est(:, i) = x_est(:, i) ./ x_est(3, i); 56 | end 57 | errors(:, 1) = sqrt(sum((x_est - x).^2, 1))'; 58 | fprintf('reproject error = %f\n\n', mean(errors(:, 1))) 59 | 60 | %% PNP P3P 61 | fprintf('---------------------------PNP P3P---------------------------\n\n') 62 | 63 | P_est = solve_pnp_p3p(X_noise(:, 1:3), x(:, 1:3)); 64 | 65 | if size(P_est, 1) ~= 0 66 | min_err = 1e10; 67 | min_i = 0; 68 | for i = 1:size(P_est, 2) 69 | [~, R_est, t_est] = KRt_from_P(P_est{i}); 70 | 71 | angle_axis = rodrigues(R); 72 | angle_axis_est = rodrigues(R_est); 73 | err = sum(sum((angle_axis - angle_axis_est).^2)) + sum((t - t_est).^2); 74 | if err < min_err 75 | min_i = i; 76 | min_err = err; 77 | end 78 | end 79 | 80 | [K_est, R_est, t_est] = KRt_from_P(P_est{min_i}); 81 | 82 | angle_axis = rodrigues(R); 83 | angle_axis_est = rodrigues(R_est); 84 | 85 | fprintf('angle_axis = \n') 86 | disp(angle_axis') 87 | fprintf('angle_axis_est = \n') 88 | disp(angle_axis_est') 89 | fprintf('t = \n') 90 | disp(t') 91 | fprintf('t_est = \n') 92 | disp(t_est') 93 | 94 | x_est = [R_est, t_est] * X; 95 | for i = 1:npoints 96 | x_est(:, i) = x_est(:, i) ./ x_est(3, i); 97 | end 98 | errors(:, 2) = sqrt(sum((x_est - x).^2, 1))'; 99 | fprintf('reproject error = %f\n\n', mean(errors(:, 2))) 100 | else 101 | fprintf('p3p failed\n\n') 102 | end 103 | 104 | %% PNP EPnP 105 | fprintf('---------------------------PNP EPnP---------------------------\n\n') 106 | 107 | P_est = solve_pnp_epnp(X_noise, x); 108 | [K_est, R_est, t_est] = KRt_from_P(P_est); 109 | 110 | angle_axis = rodrigues(R); 111 | angle_axis_est = rodrigues(R_est); 112 | 113 | fprintf('angle_axis = \n') 114 | disp(angle_axis') 115 | fprintf('angle_axis_est = \n') 116 | disp(angle_axis_est') 117 | fprintf('t = \n') 118 | disp(t') 119 | fprintf('t_est = \n') 120 | disp(t_est') 121 | 122 | x_est = [R_est, t_est] * X; 123 | for i = 1:npoints 124 | x_est(:, i) = x_est(:, i) ./ x_est(3, i); 125 | end 126 | errors(:, 3) = sqrt(sum((x_est - x).^2, 1))'; 127 | fprintf('reproject error = %f\n\n', mean(errors(:, 3))) 128 | 129 | %% PNP AP3P 130 | fprintf('---------------------------PNP AP3P---------------------------\n\n') 131 | 132 | P_est = solve_pnp_ap3p(X_noise(:, 1:3), x(:, 1:3)); 133 | 134 | if size(P_est, 1) ~= 0 135 | min_err = 1e10; 136 | min_i = 0; 137 | for i = 1:size(P_est, 2) 138 | [~, R_est, t_est] = KRt_from_P(P_est{i}); 139 | 140 | angle_axis = rodrigues(R); 141 | angle_axis_est = rodrigues(R_est); 142 | err = sum(sum((angle_axis - angle_axis_est).^2)) + sum((t - t_est).^2); 143 | if err < min_err 144 | min_i = i; 145 | min_err = err; 146 | end 147 | end 148 | 149 | [K_est, R_est, t_est] = KRt_from_P(P_est{min_i}); 150 | 151 | angle_axis = rodrigues(R); 152 | angle_axis_est = rodrigues(R_est); 153 | 154 | fprintf('angle_axis = \n') 155 | disp(angle_axis') 156 | fprintf('angle_axis_est = \n') 157 | disp(angle_axis_est') 158 | fprintf('t = \n') 159 | disp(t') 160 | fprintf('t_est = \n') 161 | disp(t_est') 162 | 163 | x_est = [R_est, t_est] * X; 164 | for i = 1:npoints 165 | x_est(:, i) = x_est(:, i) ./ x_est(3, i); 166 | end 167 | errors(:, 4) = sqrt(sum((x_est - x).^2, 1))'; 168 | fprintf('reproject error = %f\n\n', mean(errors(:, 4))) 169 | else 170 | fprintf('ap3p failed\n\n') 171 | end 172 | 173 | boxplot(errors) 174 | --------------------------------------------------------------------------------