From 19977abc52d1dd5d1ef93238c2f73f3c1229b606 Mon Sep 17 00:00:00 2001 From: shshlei Date: Fri, 18 Aug 2023 13:46:04 +0800 Subject: [PATCH 1/2] Using ExpMap --- MLPnP/MLPNP_with_COV.m | 7 +- MLPnP/MLPNP_without_COV.m | 7 +- MLPnP/MLPnP.m | 384 +++++++++++++++++------------------ MLPnP/Rodrigues2.m | 48 ++--- MLPnP/jacobians_Rodrigues.m | 329 ------------------------------ MLPnP/optim_MLPnP_GN.m | 99 ++++----- MLPnP/residualsAndJacobian.m | 30 ++- MLPnP/skew.m | 24 +++ main_ordinary_3d.m | 207 +++++++++---------- main_planar.m | 207 +++++++++---------- plot_funcs/plotOrdinary3D.m | 4 +- plot_funcs/plotPlanar3D.m | 4 +- rpnp/code3/func/xdrawgraph.m | 43 ++-- 13 files changed, 514 insertions(+), 879 deletions(-) delete mode 100644 MLPnP/jacobians_Rodrigues.m create mode 100644 MLPnP/skew.m diff --git a/MLPnP/MLPNP_with_COV.m b/MLPnP/MLPNP_with_COV.m index 865d0c1..3771381 100644 --- a/MLPnP/MLPNP_with_COV.m +++ b/MLPnP/MLPNP_with_COV.m @@ -1,6 +1,5 @@ function [R,t] = MLPNP_with_COV(XX,xx,cov) -T = MLPnP(XX, normc(xx),cov); -R = T(1:3,1:3); -t = T(1:3,4); + T = MLPnP(XX, normc(xx),cov); + R = T(1:3,1:3); + t = T(1:3,4); end - diff --git a/MLPnP/MLPNP_without_COV.m b/MLPnP/MLPNP_without_COV.m index fc85704..622cd92 100644 --- a/MLPnP/MLPNP_without_COV.m +++ b/MLPnP/MLPNP_without_COV.m @@ -1,6 +1,5 @@ function [R,t] = MLPNP_without_COV(XX, xx, cov) -T = MLPnP(XX, normc(xx)); -R = T(1:3,1:3); -t = T(1:3,4); + T = MLPnP(XX, normc(xx)); + R = T(1:3,1:3); + t = T(1:3,4); end - diff --git a/MLPnP/MLPnP.m b/MLPnP/MLPnP.m index c18c81f..781c645 100644 --- a/MLPnP/MLPnP.m +++ b/MLPnP/MLPnP.m @@ -26,7 +26,7 @@ % pages = "131-138"} %% MLPnP - Maximum Likelihood Perspective-N-Point -% input: 1. points3D - a 3xN matrix of N 3D points in the object coordinate system +% input: 1. points3D - a 3xN matrix of N 3D points in the world coordinate system % 2. v - a 3xN matrix of N bearing vectors (camera rays) % ||v|| = 1 % 3. cov - if covariance information of bearing vectors if @@ -39,212 +39,210 @@ % bearing vector transformation (inverse calibration % matrix K. Details in the paper. % output: 1. T - 4x4 transformation matrix [R T;0 0 0 1] -% 2. statistics - contains statistics after GN refinement, see -% optim_GN.m for details -function [T, statistics] = MLPnP(points3D, v, cov) - -use_cov = 1; -% if cov is not given don't use it -if nargin < 3 - use_cov = 0; -end +function [T] = MLPnP(points3D, v, cov) + use_cov = 1; + % if cov is not given don't use it + if nargin < 3 + use_cov = 0; + end -nrPts = size(points3D,2); -% matrix of null space vectors r and s -r = zeros(3,nrPts); -s = zeros(3,nrPts); -cov_reduced = zeros(2,2,nrPts); + nrPts = size(points3D,2); + % matrix of null space vectors r and s + r = zeros(3,nrPts); + s = zeros(3,nrPts); + cov_reduced = zeros(2,2,nrPts); -% test planarity, only works well if the scene is really planar -% quasi-planar won't work very well -S = points3D*points3D'; -[eigRot,~] = eig(S); -planar = 0; -% create full design matrix -A = zeros(nrPts,12); -if (rank(S) == 2) - planar = 1; - points3D1 = eigRot'*(points3D); - points3Dn = [points3D1;ones(1,nrPts)]; - % create reduced design matrix - A = zeros(nrPts,9); -else - points3Dn = [points3D;ones(1,nrPts)]; -end + % compute null spaces of bearing vector v: null(v') + for i=1:nrPts + null_2d = null(v(1:3,i)'); + r(:,i) = null_2d(:,1); + s(:,i) = null_2d(:,2); + if use_cov + tmp = reshape(cov(:,i),3,3); + cov_reduced(:,:,i) = (null_2d'*tmp*null_2d)^-1; + end + end -% compute null spaces of bearing vector v: null(v') -for i=1:nrPts - null_2d = null(v(1:3,i)'); - r(:,i) = null_2d(:,1); - s(:,i) = null_2d(:,2); - if use_cov - tmp = reshape(cov(:,i),3,3); - cov_reduced(:,:,i) = (null_2d'*tmp*null_2d)^-1; + % test planarity, only works well if the scene is really planar + % quasi-planar won't work very well + S = points3D*points3D'; + [eigRot,~] = eig(S); + planar = 0; + % create full design matrix + A = zeros(nrPts,12); + if (rank(S) == 2) + planar = 1; + points3D1 = eigRot'*(points3D); + points3Dn = [points3D1;ones(1,nrPts)]; + % create reduced design matrix + A = zeros(nrPts,9); + else + points3Dn = [points3D;ones(1,nrPts)]; end -end -% stochastic model -Kll = eye(2*nrPts,2*nrPts); -% if (normalize) -% points3Dn = normc(points3Dn); -% end -if planar % build reduces system - for i=1:nrPts - if (use_cov) - Kll(2*i-1:2*i,2*i-1:2*i) = cov_reduced(:,:,i); + % stochastic model + Kll = eye(2*nrPts,2*nrPts); + if planar % build reduces system + for i=1:nrPts + if (use_cov) + Kll(2*i-1:2*i,2*i-1:2*i) = cov_reduced(:,:,i); + end + % r12 + A (2*i-1,1) = r(1,i)*points3Dn(2,i); + A (2*i,1) = s(1,i)*points3Dn(2,i); + % r13 + A (2*i-1,2) = r(1,i)*points3Dn(3,i); + A (2*i,2) = s(1,i)*points3Dn(3,i); + % r22 + A (2*i-1,3) = r(2,i)*points3Dn(2,i); + A (2*i,3) = s(2,i)*points3Dn(2,i); + % r23 + A (2*i-1,4) = r(2,i)*points3Dn(3,i); + A (2*i,4) = s(2,i)*points3Dn(3,i); + % r31 + A (2*i-1,5) = r(3,i)*points3Dn(2,i); + A (2*i,5) = s(3,i)*points3Dn(2,i); + % r32 + A (2*i-1,6) = r(3,i)*points3Dn(3,i); + A (2*i,6) = s(3,i)*points3Dn(3,i); + % t1 + A (2*i-1,7) = r(1,i); + A (2*i,7) = s(1,i); + % t2 + A (2*i-1,8) = r(2,i); + A (2*i,8) = s(2,i); + % t3 + A (2*i-1,9) = r(3,i); + A (2*i,9) = s(3,i); + end + else % build full system + for i=1:nrPts + if (use_cov) + Kll(2*i-1:2*i,2*i-1:2*i) = cov_reduced(:,:,i); + end + % r11 + A (2*i-1,1) = r(1,i)*points3Dn(1,i); + A (2*i,1) = s(1,i)*points3Dn(1,i); + % r12 + A (2*i-1,2) = r(1,i)*points3Dn(2,i); + A (2*i,2) = s(1,i)*points3Dn(2,i); + % r13 + A (2*i-1,3) = r(1,i)*points3Dn(3,i); + A (2*i,3) = s(1,i)*points3Dn(3,i); + % r21 + A (2*i-1,4) = r(2,i)*points3Dn(1,i); + A (2*i,4) = s(2,i)*points3Dn(1,i); + % r22 + A (2*i-1,5) = r(2,i)*points3Dn(2,i); + A (2*i,5) = s(2,i)*points3Dn(2,i); + % r23 + A (2*i-1,6) = r(2,i)*points3Dn(3,i); + A (2*i,6) = s(2,i)*points3Dn(3,i); + % r31 + A (2*i-1,7) = r(3,i)*points3Dn(1,i); + A (2*i,7) = s(3,i)*points3Dn(1,i); + % r32 + A (2*i-1,8) = r(3,i)*points3Dn(2,i); + A (2*i,8) = s(3,i)*points3Dn(2,i); + % r33 + A (2*i-1,9) = r(3,i)*points3Dn(3,i); + A (2*i,9) = s(3,i)*points3Dn(3,i); + % t1 + A (2*i-1,10) = r(1,i); + A (2*i,10) = s(1,i); + % t2 + A (2*i-1,11) = r(2,i); + A (2*i,11) = s(2,i); + % t3 + A (2*i-1,12) = r(3,i); + A (2*i,12) = s(3,i); end - % r12 - A (2*i-1,1) = r(1,i)*points3Dn(2,i); - A (2*i,1) = s(1,i)*points3Dn(2,i); - % r13 - A (2*i-1,2) = r(1,i)*points3Dn(3,i); - A (2*i,2) = s(1,i)*points3Dn(3,i); - % r22 - A (2*i-1,3) = r(2,i)*points3Dn(2,i); - A (2*i,3) = s(2,i)*points3Dn(2,i); - % r23 - A (2*i-1,4) = r(2,i)*points3Dn(3,i); - A (2*i,4) = s(2,i)*points3Dn(3,i); - % r31 - A (2*i-1,5) = r(3,i)*points3Dn(2,i); - A (2*i,5) = s(3,i)*points3Dn(2,i); - % r32 - A (2*i-1,6) = r(3,i)*points3Dn(3,i); - A (2*i,6) = s(3,i)*points3Dn(3,i); - % t1 - A (2*i-1,7) = r(1,i); - A (2*i,7) = s(1,i); - % t2 - A (2*i-1,8) = r(2,i); - A (2*i,8) = s(2,i); - % t3 - A (2*i-1,9) = r(3,i); - A (2*i,9) = s(3,i); + end - end -else % build full system - for i=1:nrPts - if (use_cov) - Kll(2*i-1:2*i,2*i-1:2*i) = cov_reduced(:,:,i); - end - % r11 - A (2*i-1,1) = r(1,i)*points3Dn(1,i); - A (2*i,1) = s(1,i)*points3Dn(1,i); - % r12 - A (2*i-1,2) = r(1,i)*points3Dn(2,i); - A (2*i,2) = s(1,i)*points3Dn(2,i); - % r13 - A (2*i-1,3) = r(1,i)*points3Dn(3,i); - A (2*i,3) = s(1,i)*points3Dn(3,i); - % r21 - A (2*i-1,4) = r(2,i)*points3Dn(1,i); - A (2*i,4) = s(2,i)*points3Dn(1,i); - % r22 - A (2*i-1,5) = r(2,i)*points3Dn(2,i); - A (2*i,5) = s(2,i)*points3Dn(2,i); - % r23 - A (2*i-1,6) = r(2,i)*points3Dn(3,i); - A (2*i,6) = s(2,i)*points3Dn(3,i); - % r31 - A (2*i-1,7) = r(3,i)*points3Dn(1,i); - A (2*i,7) = s(3,i)*points3Dn(1,i); - % r32 - A (2*i-1,8) = r(3,i)*points3Dn(2,i); - A (2*i,8) = s(3,i)*points3Dn(2,i); - % r33 - A (2*i-1,9) = r(3,i)*points3Dn(3,i); - A (2*i,9) = s(3,i)*points3Dn(3,i); - % t1 - A (2*i-1,10) = r(1,i); - A (2*i,10) = s(1,i); - % t2 - A (2*i-1,11) = r(2,i); - A (2*i,11) = s(2,i); - % t3 - A (2*i-1,12) = r(3,i); - A (2*i,12) = s(3,i); + % do least squares AtPAx=0 + if use_cov + b = A'*Kll*A; + else + b = A'*A; end -end + [~,~,v1] = svd(b); -% do least squares AtPAx=0 -b = A'*A; -[~,~,v1] = svd(b); + if planar + P = zeros(3,3); + P(:,2:3) = reshape(v1(1:6,end),2,3)'; + scalefact = sqrt(abs(norm(P(:,2))*norm(P(:,3)))); + P(:,1) = cross(P(:,2),P(:,3)); -if planar - tout1 = v1(7:9,end); - P = zeros(3,3); - P(:,2:3) = reshape(v1(1:6,end),2,3)'; - scalefact = sqrt(abs(norm(P(:,2))*norm(P(:,3)))); - P(:,1) = cross(P(:,2),P(:,3)); - P = P'; - %SVD to find the best rotation matrix in the Frobenius sense - [U2,~,V2] = svd(P(1:3,1:3)); - R = U2*V2'; - if det(R) < 0 - R = -1*R; - end - % rotate solution back (see paper) - R = eigRot*R; - % recover translation - tout = (tout1./scalefact); - R = -R'; - - R1 = [R(:,1) R(:,2) R(:,3)]; - R2 = [-R(:,1) -R(:,2) R(:,3)]; - Ts = zeros(4,4,4); - Ts(:,:,1) = [R1 tout;0 0 0 1]; - Ts(:,:,2) = [R1 -tout;0 0 0 1]; - Ts(:,:,3) = [R2 tout;0 0 0 1]; - Ts(:,:,4) = [R2 -tout;0 0 0 1]; - % find the best solution with 6 correspondences - diff1 = zeros(4,1); - for te = 1:6 - for ba = 1:4 - testres1 = Ts(:,:,ba)*[points3D(:,te);1]; - testres11 = normc(testres1(1:3)); - diff1(ba) = diff1(ba) + (1-dot(testres11,v(:,te))); + %SVD to find the best rotation matrix in the Frobenius sense + [U2,~,V2] = svd(P(1:3,1:3)); + R = U2*V2'; + if det(R) < 0 + R = -1*R; end - end - [~,idx] = min(diff1); - T = Ts(:,:,idx); -else - tout1 = v1(10:12,end); - P = reshape(v1(1:9,end),3,3); - scalefact = (abs(norm(P(:,1))*norm(P(:,2))*norm(P(:,3))))^(1/3); - %SVD to find the best rotation matrix in the Frobenius sense - [U2,~,V2] = svd(P(1:3,1:3)); - R = U2*V2'; - if det(R) < 0 - R = -1*R; - end - % recover translation - tout = R*(tout1./scalefact); - T1 = [R tout;0 0 0 1]^-1; - T2 = [R -tout;0 0 0 1]^-1; - diff1 = 0; - diff2 = 0; - % find the best solution with 6 correspondences - for te = 1:6 - testres1 = T1*[points3D(:,te);1]; - testres2 = T2*[points3D(:,te);1]; - testres1 = normc(testres1(1:3)); - testres2 = normc(testres2(1:3)); + % rotate solution back (see paper) + R = R*eigRot'; % cw - diff1 = diff1+(1-dot(testres1,v(:,te))); - diff2 = diff2+(1-dot(testres2,v(:,te))); - end - if diff1 < diff2 - T = T1(1:3,1:4); + % recover translation + tout1 = v1(7:9,end); + tout = (tout1./scalefact); + + R1 = [-R(:,1) -R(:,2) -R(:,3)]; + R2 = [R(:,1) R(:,2) -R(:,3)]; + Ts = zeros(4,4,4); + Ts(:,:,1) = [R1 tout;0 0 0 1]; + Ts(:,:,2) = [R1 -tout;0 0 0 1]; + Ts(:,:,3) = [R2 tout;0 0 0 1]; + Ts(:,:,4) = [R2 -tout;0 0 0 1]; + % find the best solution with 6 correspondences + diff1 = zeros(4,1); + for te = 1:6 + for ba = 1:4 + testres1 = Ts(:,:,ba)*[points3D(:,te);1]; + testres11 = normc(testres1(1:3)); + diff1(ba) = diff1(ba) + (1-dot(testres11,v(:,te))); + end + end + [~,idx] = min(diff1); + T = Ts(:,:,idx); else - T = T2(1:3,1:4); + tout1 = v1(10:12,end); + P = reshape(v1(1:9,end),3,3)'; + scalefact = (abs(norm(P(:,1))*norm(P(:,2))*norm(P(:,3))))^(1/3); + %SVD to find the best rotation matrix in the Frobenius sense + [U2,~,V2] = svd(P(1:3,1:3)); + R = U2*V2'; %cw + if det(R) < 0 + R = -1*R; + end + % recover translation + tout = tout1./scalefact; + T1 = [R tout;0 0 0 1]; + T2 = [R -tout;0 0 0 1]; + diff1 = 0; + diff2 = 0; + % find the best solution with 6 correspondences + for te = 1:6 + testres1 = T1*[points3D(:,te);1]; + testres2 = T2*[points3D(:,te);1]; + testres1 = normc(testres1(1:3)); + testres2 = normc(testres2(1:3)); + + diff1 = diff1+(1-dot(testres1,v(:,te))); + diff2 = diff2+(1-dot(testres2,v(:,te))); + end + if diff1 < diff2 + T = T1(1:3,1:4); + else + T = T2(1:3,1:4); + end end -end -optimFlags.epsP = 1e-6; -optimFlags.epsF = 1e-6; -optimFlags.maxit = 5; -optimFlags.tau = 1e-4; -[T, statistics] = optim_MLPnP_GN(T, points3D, r, s, Kll, optimFlags); -end + optimFlags.epsP = 1e-6; + optimFlags.epsF = 1e-6; + optimFlags.maxit = 5; + optimFlags.tau = 1e-4; + [Rout, tout] = optim_MLPnP_GN(T(1:3,1:3), T(1:3,4), points3D, r, s, Kll, optimFlags); + T = [Rout tout]; +end diff --git a/MLPnP/Rodrigues2.m b/MLPnP/Rodrigues2.m index 9d79c50..71d2b9f 100644 --- a/MLPnP/Rodrigues2.m +++ b/MLPnP/Rodrigues2.m @@ -19,33 +19,27 @@ function R2 = Rodrigues2(R1) -[r,c] = size(R1); + [r,c] = size(R1); -%% Rodrigues Rotation Vector to Rotation Matrix -if ((r == 3) && (c == 1)) || ((r == 1) && (c == 3)) - wx = [ 0 -R1(3) R1(2); - R1(3) 0 -R1(1); - -R1(2) R1(1) 0 ]; - - omega_norm = sqrt(R1(1)^2 + R1(2)^2 + R1(3)^2); - - if (omega_norm < eps) - R2 = eye(3); - else - R2 = eye(3) + ... - sin(omega_norm)/omega_norm*wx + ... - (1-cos(omega_norm))/omega_norm^2*wx^2; - end - -%% Rotation Matrix to Rodrigues Rotation Vector -elseif (r == 3) && (c == 3) - w_norm = acos((trace(R1)-1)/2); - if (w_norm < eps) - R2 = [0 0 0]'; - else - R2 = 1/(2*sin(w_norm)) * ... - [R1(3,2)-R1(2,3);R1(1,3)-R1(3,1);R1(2,1)-R1(1,2)]*w_norm; + %% Rodrigues Rotation Vector to Rotation Matrix + if ((r == 3) && (c == 1)) || ((r == 1) && (c == 3)) + omega_norm = norm(R1); + if (omega_norm < eps) + R2 = eye(3); + else + wx = skew(R1); + R2 = eye(3) + ... + sin(omega_norm)/omega_norm*wx + ... + (1-cos(omega_norm))/omega_norm^2*wx^2; + end + %% Rotation Matrix to Rodrigues Rotation Vector + elseif (r == 3) && (c == 3) + w_norm = acos((trace(R1)-1)/2); + if (w_norm < eps) + R2 = [0 0 0]'; + else + R2 = 1/(2*sin(w_norm)) * ... + [R1(3,2)-R1(2,3);R1(1,3)-R1(3,1);R1(2,1)-R1(1,2)]*w_norm; + end end end - - diff --git a/MLPnP/jacobians_Rodrigues.m b/MLPnP/jacobians_Rodrigues.m deleted file mode 100644 index 1722421..0000000 --- a/MLPnP/jacobians_Rodrigues.m +++ /dev/null @@ -1,329 +0,0 @@ -% Steffen Urban email: urbste@googlemail.com -% Copyright (C) 2016 Steffen Urban -% -% This program is free software; you can redistribute it and/or modify -% it under the terms of the GNU General Public License as published by -% the Free Software Foundation; either version 2 of the License, or -% (at your option) any later version. -% -% This program is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU General Public License along -% with this program; if not, write to the Free Software Foundation, Inc., -% 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -% 28.06.2016 Steffen Urban - -% file was created with the Matlab symbolic toolbox -% todo: jacobians would probably a lot easier if I'd use expmap and -% optimize near the identity... -% would need to change the update step from x+dx to exp(dx)*exp(x) or sth -function jacs = jacobians_Rodrigues(X1,Y1,Z1,r1,r2,r3,s1,s2,s3,t1,t2,t3,w1,w2,w3) -t5 = w1.^2; -t6 = w2.^2; -t7 = w3.^2; -t8 = t5+t6+t7; -t9 = sqrt(t8); -t10 = sin(t9); -t11 = 1.0./sqrt(t8); -t12 = cos(t9); -t13 = t12-1.0; -t14 = 1.0./t8; -t16 = t10.*t11.*w3; -t17 = t13.*t14.*w1.*w2; -t19 = t10.*t11.*w2; -t20 = t13.*t14.*w1.*w3; -t24 = t6+t7; -t27 = t16+t17; -t28 = Y1.*t27; -t29 = t19-t20; -t30 = Z1.*t29; -t31 = t13.*t14.*t24; -t32 = t31+1.0; -t33 = X1.*t32; -t15 = t1-t28+t30+t33; -t21 = t10.*t11.*w1; -t22 = t13.*t14.*w2.*w3; -t45 = t5+t7; -t53 = t16-t17; -t54 = X1.*t53; -t55 = t21+t22; -t56 = Z1.*t55; -t57 = t13.*t14.*t45; -t58 = t57+1.0; -t59 = Y1.*t58; -t18 = t2+t54-t56+t59; -t34 = t5+t6; -t38 = t19+t20; -t39 = X1.*t38; -t40 = t21-t22; -t41 = Y1.*t40; -t42 = t13.*t14.*t34; -t43 = t42+1.0; -t44 = Z1.*t43; -t23 = t3-t39+t41+t44; -t25 = 1.0./t8.^(3.0./2.0); -t26 = 1.0./t8.^2; -t35 = t12.*t14.*w1.*w2; -t36 = t5.*t10.*t25.*w3; -t37 = t5.*t13.*t26.*w3.*2.0; -t46 = t10.*t25.*w1.*w3; -t47 = t5.*t10.*t25.*w2; -t48 = t5.*t13.*t26.*w2.*2.0; -t49 = t10.*t11; -t50 = t5.*t12.*t14; -t51 = t13.*t26.*w1.*w2.*w3.*2.0; -t52 = t10.*t25.*w1.*w2.*w3; -t60 = t15.^2; -t61 = t18.^2; -t62 = t23.^2; -t63 = t60+t61+t62; -t64 = t5.*t10.*t25; -t65 = 1.0./sqrt(t63); -t66 = Y1.*r2.*t6; -t67 = Z1.*r3.*t7; -t68 = r1.*t1.*t5; -t69 = r1.*t1.*t6; -t70 = r1.*t1.*t7; -t71 = r2.*t2.*t5; -t72 = r2.*t2.*t6; -t73 = r2.*t2.*t7; -t74 = r3.*t3.*t5; -t75 = r3.*t3.*t6; -t76 = r3.*t3.*t7; -t77 = X1.*r1.*t5; -t78 = X1.*r2.*w1.*w2; -t79 = X1.*r3.*w1.*w3; -t80 = Y1.*r1.*w1.*w2; -t81 = Y1.*r3.*w2.*w3; -t82 = Z1.*r1.*w1.*w3; -t83 = Z1.*r2.*w2.*w3; -t84 = X1.*r1.*t6.*t12; -t85 = X1.*r1.*t7.*t12; -t86 = Y1.*r2.*t5.*t12; -t87 = Y1.*r2.*t7.*t12; -t88 = Z1.*r3.*t5.*t12; -t89 = Z1.*r3.*t6.*t12; -t90 = X1.*r2.*t9.*t10.*w3; -t91 = Y1.*r3.*t9.*t10.*w1; -t92 = Z1.*r1.*t9.*t10.*w2; -t102 = X1.*r3.*t9.*t10.*w2; -t103 = Y1.*r1.*t9.*t10.*w3; -t104 = Z1.*r2.*t9.*t10.*w1; -t105 = X1.*r2.*t12.*w1.*w2; -t106 = X1.*r3.*t12.*w1.*w3; -t107 = Y1.*r1.*t12.*w1.*w2; -t108 = Y1.*r3.*t12.*w2.*w3; -t109 = Z1.*r1.*t12.*w1.*w3; -t110 = Z1.*r2.*t12.*w2.*w3; -t93 = t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+... - t83+t84+t85+t86+t87+t88+t89+t90+t91+t92-t102-t103-t104-t105-t106-t107-t108-t109-t110; -t94 = t10.*t25.*w1.*w2; -t95 = t6.*t10.*t25.*w3; -t96 = t6.*t13.*t26.*w3.*2.0; -t97 = t12.*t14.*w2.*w3; -t98 = t6.*t10.*t25.*w1; -t99 = t6.*t13.*t26.*w1.*2.0; -t100 = t6.*t10.*t25; -t101 = 1.0./t63.^(3.0./2.0); -t111 = t6.*t12.*t14; -t112 = t10.*t25.*w2.*w3; -t113 = t12.*t14.*w1.*w3; -t114 = t7.*t10.*t25.*w2; -t115 = t7.*t13.*t26.*w2.*2.0; -t116 = t7.*t10.*t25.*w1; -t117 = t7.*t13.*t26.*w1.*2.0; -t118 = t7.*t12.*t14; -t119 = t13.*t24.*t26.*w1.*2.0; -t120 = t10.*t24.*t25.*w1; -t121 = t119+t120; -t122 = t13.*t26.*t34.*w1.*2.0; -t123 = t10.*t25.*t34.*w1; -t131 = t13.*t14.*w1.*2.0; -t124 = t122+t123-t131; -t139 = t13.*t14.*w3; -t125 = -t35+t36+t37+t94-t139; -t126 = X1.*t125; -t127 = t49+t50+t51+t52-t64; -t128 = Y1.*t127; -t129 = t126+t128-Z1.*t124; -t130 = t23.*t129.*2.0; -t132 = t13.*t26.*t45.*w1.*2.0; -t133 = t10.*t25.*t45.*w1; -t138 = t13.*t14.*w2; -t134 = -t46+t47+t48+t113-t138; -t135 = X1.*t134; -t136 = -t49-t50+t51+t52+t64; -t137 = Z1.*t136; -t140 = X1.*s1.*t5; -t141 = Y1.*s2.*t6; -t142 = Z1.*s3.*t7; -t143 = s1.*t1.*t5; -t144 = s1.*t1.*t6; -t145 = s1.*t1.*t7; -t146 = s2.*t2.*t5; -t147 = s2.*t2.*t6; -t148 = s2.*t2.*t7; -t149 = s3.*t3.*t5; -t150 = s3.*t3.*t6; -t151 = s3.*t3.*t7; -t152 = X1.*s2.*w1.*w2; -t153 = X1.*s3.*w1.*w3; -t154 = Y1.*s1.*w1.*w2; -t155 = Y1.*s3.*w2.*w3; -t156 = Z1.*s1.*w1.*w3; -t157 = Z1.*s2.*w2.*w3; -t158 = X1.*s1.*t6.*t12; -t159 = X1.*s1.*t7.*t12; -t160 = Y1.*s2.*t5.*t12; -t161 = Y1.*s2.*t7.*t12; -t162 = Z1.*s3.*t5.*t12; -t163 = Z1.*s3.*t6.*t12; -t164 = X1.*s2.*t9.*t10.*w3; -t165 = Y1.*s3.*t9.*t10.*w1; -t166 = Z1.*s1.*t9.*t10.*w2; -t183 = X1.*s3.*t9.*t10.*w2; -t184 = Y1.*s1.*t9.*t10.*w3; -t185 = Z1.*s2.*t9.*t10.*w1; -t186 = X1.*s2.*t12.*w1.*w2; -t187 = X1.*s3.*t12.*w1.*w3; -t188 = Y1.*s1.*t12.*w1.*w2; -t189 = Y1.*s3.*t12.*w2.*w3; -t190 = Z1.*s1.*t12.*w1.*w3; -t191 = Z1.*s2.*t12.*w2.*w3; -t167 = t140+t141+t142+t143+t144+t145+t146+t147+t148+t149+t150+t151+t152+... - t153+t154+t155+t156+t157+t158+t159+t160+t161+t162+t163+t164+t165+t166-... - t183-t184-t185-t186-t187-t188-t189-t190-t191; -t168 = t13.*t26.*t45.*w2.*2.0; -t169 = t10.*t25.*t45.*w2; -t170 = t168+t169; -t171 = t13.*t26.*t34.*w2.*2.0; -t172 = t10.*t25.*t34.*w2; -t176 = t13.*t14.*w2.*2.0; -t173 = t171+t172-t176; -t174 = -t49+t51+t52+t100-t111; -t175 = X1.*t174; -t177 = t13.*t24.*t26.*w2.*2.0; -t178 = t10.*t24.*t25.*w2; -t192 = t13.*t14.*w1; -t179 = -t97+t98+t99+t112-t192; -t180 = Y1.*t179; -t181 = t49+t51+t52-t100+t111; -t182 = Z1.*t181; -t193 = t13.*t26.*t34.*w3.*2.0; -t194 = t10.*t25.*t34.*w3; -t195 = t193+t194; -t196 = t13.*t26.*t45.*w3.*2.0; -t197 = t10.*t25.*t45.*w3; -t200 = t13.*t14.*w3.*2.0; -t198 = t196+t197-t200; -t199 = t7.*t10.*t25; -t201 = t13.*t24.*t26.*w3.*2.0; -t202 = t10.*t24.*t25.*w3; -t203 = -t49+t51+t52-t118+t199; -t204 = Y1.*t203; -t205 = t1.*2.0; -t206 = Z1.*t29.*2.0; -t207 = X1.*t32.*2.0; -t208 = t205+t206+t207-Y1.*t27.*2.0; -t209 = t2.*2.0; -t210 = X1.*t53.*2.0; -t211 = Y1.*t58.*2.0; -t212 = t209+t210+t211-Z1.*t55.*2.0; -t213 = t3.*2.0; -t214 = Y1.*t40.*2.0; -t215 = Z1.*t43.*2.0; -t216 = t213+t214+t215-X1.*t38.*2.0; -jacs = reshape([t14.*t65.*(X1.*r1.*w1.*2.0+X1.*r2.*w2+X1.*r3.*w3+Y1.*r1.*w2+... - Z1.*r1.*w3+r1.*t1.*w1.*2.0+r2.*t2.*w1.*2.0+r3.*t3.*w1.*2.0+Y1.*r3.*t5.*t12+... - Y1.*r3.*t9.*t10-Z1.*r2.*t5.*t12-Z1.*r2.*t9.*t10-X1.*r2.*t12.*w2-X1.*r3.*t12.*w3-... - Y1.*r1.*t12.*w2+Y1.*r2.*t12.*w1.*2.0-Z1.*r1.*t12.*w3+Z1.*r3.*t12.*w1.*2.0+... - Y1.*r3.*t5.*t10.*t11-Z1.*r2.*t5.*t10.*t11+X1.*r2.*t12.*w1.*w3-... - X1.*r3.*t12.*w1.*w2-Y1.*r1.*t12.*w1.*w3+Z1.*r1.*t12.*w1.*w2-... - Y1.*r1.*t10.*t11.*w1.*w3+Z1.*r1.*t10.*t11.*w1.*w2-... - X1.*r1.*t6.*t10.*t11.*w1-X1.*r1.*t7.*t10.*t11.*w1+X1.*r2.*t5.*t10.*t11.*w2+... - X1.*r3.*t5.*t10.*t11.*w3+Y1.*r1.*t5.*t10.*t11.*w2-Y1.*r2.*t5.*t10.*t11.*w1-... - Y1.*r2.*t7.*t10.*t11.*w1+Z1.*r1.*t5.*t10.*t11.*w3-Z1.*r3.*t5.*t10.*t11.*w1-... - Z1.*r3.*t6.*t10.*t11.*w1+X1.*r2.*t10.*t11.*w1.*w3-X1.*r3.*t10.*t11.*w1.*w2+... - Y1.*r3.*t10.*t11.*w1.*w2.*w3+Z1.*r2.*t10.*t11.*w1.*w2.*w3)-t26.*t65.*t93.*w1.*2.0-... - t14.*t93.*t101.*(t130+t15.*(-X1.*t121+Y1.*(t46+t47+t48-t13.*t14.*w2-t12.*t14.*w1.*w3)+... - Z1.*(t35+t36+t37-t13.*t14.*w3-t10.*t25.*w1.*w2)).*2.0+... - t18.*(t135+t137-Y1.*(t132+t133-t13.*t14.*w1.*2.0)).*2.0).*(1.0./2.0),t14.*t65.*(X1.*s1.*w1.*2.0+X1.*s2.*w2+... - X1.*s3.*w3+Y1.*s1.*w2+Z1.*s1.*w3+s1.*t1.*w1.*2.0+s2.*t2.*w1.*2.0+... - s3.*t3.*w1.*2.0+Y1.*s3.*t5.*t12+Y1.*s3.*t9.*t10-Z1.*s2.*t5.*t12-Z1.*s2.*t9.*t10-X1.*s2.*t12.*w2-... - X1.*s3.*t12.*w3-Y1.*s1.*t12.*w2+Y1.*s2.*t12.*w1.*2.0-Z1.*s1.*t12.*w3+... - Z1.*s3.*t12.*w1.*2.0+Y1.*s3.*t5.*t10.*t11-Z1.*s2.*t5.*t10.*t11+... - X1.*s2.*t12.*w1.*w3-X1.*s3.*t12.*w1.*w2-Y1.*s1.*t12.*w1.*w3+... - Z1.*s1.*t12.*w1.*w2+X1.*s2.*t10.*t11.*w1.*w3-X1.*s3.*t10.*t11.*w1.*w2-... - Y1.*s1.*t10.*t11.*w1.*w3+Z1.*s1.*t10.*t11.*w1.*w2-X1.*s1.*t6.*t10.*t11.*w1-... - X1.*s1.*t7.*t10.*t11.*w1+X1.*s2.*t5.*t10.*t11.*w2+X1.*s3.*t5.*t10.*t11.*w3+... - Y1.*s1.*t5.*t10.*t11.*w2-Y1.*s2.*t5.*t10.*t11.*w1-Y1.*s2.*t7.*t10.*t11.*w1+... - Z1.*s1.*t5.*t10.*t11.*w3-Z1.*s3.*t5.*t10.*t11.*w1-Z1.*s3.*t6.*t10.*t11.*w1+... - Y1.*s3.*t10.*t11.*w1.*w2.*w3+Z1.*s2.*t10.*t11.*w1.*w2.*w3)-... - t14.*t101.*t167.*(t130+t15.*(Y1.*(t46+t47+t48-t113-t138)+... - Z1.*(t35+t36+t37-t94-t139)-X1.*t121).*2.0+t18.*(t135+t137-... - Y1.*(-t131+t132+t133)).*2.0).*(1.0./2.0)-t26.*t65.*t167.*w1.*2.0,t14.*t65.*(X1.*r2.*w1+... - Y1.*r1.*w1+Y1.*r2.*w2.*2.0+Y1.*r3.*w3+Z1.*r2.*w3+r1.*t1.*w2.*2.0+... - r2.*t2.*w2.*2.0+r3.*t3.*w2.*2.0-X1.*r3.*t6.*t12-X1.*r3.*t9.*t10+... - Z1.*r1.*t6.*t12+Z1.*r1.*t9.*t10+X1.*r1.*t12.*w2.*2.0-X1.*r2.*t12.*w1-... - Y1.*r1.*t12.*w1-Y1.*r3.*t12.*w3-Z1.*r2.*t12.*w3+Z1.*r3.*t12.*w2.*2.0-... - X1.*r3.*t6.*t10.*t11+Z1.*r1.*t6.*t10.*t11+X1.*r2.*t12.*w2.*w3-Y1.*r1.*t12.*w2.*w3+... - Y1.*r3.*t12.*w1.*w2-Z1.*r2.*t12.*w1.*w2-Y1.*r1.*t10.*t11.*w2.*w3+... - Y1.*r3.*t10.*t11.*w1.*w2-Z1.*r2.*t10.*t11.*w1.*w2-X1.*r1.*t6.*t10.*t11.*w2+... - X1.*r2.*t6.*t10.*t11.*w1-X1.*r1.*t7.*t10.*t11.*w2+Y1.*r1.*t6.*t10.*t11.*w1-... - Y1.*r2.*t5.*t10.*t11.*w2-Y1.*r2.*t7.*t10.*t11.*w2+Y1.*r3.*t6.*t10.*t11.*w3-... - Z1.*r3.*t5.*t10.*t11.*w2+Z1.*r2.*t6.*t10.*t11.*w3-Z1.*r3.*t6.*t10.*t11.*w2+... - X1.*r2.*t10.*t11.*w2.*w3+X1.*r3.*t10.*t11.*w1.*w2.*w3+Z1.*r1.*t10.*t11.*w1.*w2.*w3)-... - t26.*t65.*t93.*w2.*2.0-t14.*t93.*t101.*(t18.*(Z1.*(-t35+t94+t95+t96-t13.*t14.*w3)-... - Y1.*t170+X1.*(t97+t98+t99-t13.*t14.*w1-t10.*t25.*w2.*w3)).*2.0+... - t15.*(t180+t182-X1.*(t177+t178-t13.*t14.*w2.*2.0)).*2.0+t23.*(t175+... - Y1.*(t35-t94+t95+t96-t13.*t14.*w3)-Z1.*t173).*2.0).*(1.0./2.0),t14.*t65.*(X1.*s2.*w1+... - Y1.*s1.*w1+Y1.*s2.*w2.*2.0+Y1.*s3.*w3+Z1.*s2.*w3+s1.*t1.*w2.*2.0+s2.*t2.*w2.*2.0+... - s3.*t3.*w2.*2.0-X1.*s3.*t6.*t12-X1.*s3.*t9.*t10+Z1.*s1.*t6.*t12+Z1.*s1.*t9.*t10+... - X1.*s1.*t12.*w2.*2.0-X1.*s2.*t12.*w1-Y1.*s1.*t12.*w1-Y1.*s3.*t12.*w3-Z1.*s2.*t12.*w3+... - Z1.*s3.*t12.*w2.*2.0-X1.*s3.*t6.*t10.*t11+Z1.*s1.*t6.*t10.*t11+X1.*s2.*t12.*w2.*w3-... - Y1.*s1.*t12.*w2.*w3+Y1.*s3.*t12.*w1.*w2-Z1.*s2.*t12.*w1.*w2+X1.*s2.*t10.*t11.*w2.*w3-... - Y1.*s1.*t10.*t11.*w2.*w3+Y1.*s3.*t10.*t11.*w1.*w2-Z1.*s2.*t10.*t11.*w1.*w2-... - X1.*s1.*t6.*t10.*t11.*w2+X1.*s2.*t6.*t10.*t11.*w1-X1.*s1.*t7.*t10.*t11.*w2+... - Y1.*s1.*t6.*t10.*t11.*w1-Y1.*s2.*t5.*t10.*t11.*w2-Y1.*s2.*t7.*t10.*t11.*w2+... - Y1.*s3.*t6.*t10.*t11.*w3-Z1.*s3.*t5.*t10.*t11.*w2+Z1.*s2.*t6.*t10.*t11.*w3-... - Z1.*s3.*t6.*t10.*t11.*w2+X1.*s3.*t10.*t11.*w1.*w2.*w3+Z1.*s1.*t10.*t11.*w1.*w2.*w3)-... - t26.*t65.*t167.*w2.*2.0-t14.*t101.*t167.*(t18.*(X1.*(t97+t98+t99-t112-t192)+... - Z1.*(-t35+t94+t95+t96-t139)-Y1.*t170).*2.0+t15.*(t180+t182-X1.*(-t176+t177+t178)).*2.0+... - t23.*(t175+Y1.*(t35-t94+t95+t96-t139)-Z1.*t173).*2.0).*(1.0./2.0),t14.*t65.*(X1.*r3.*w1+... - Y1.*r3.*w2+Z1.*r1.*w1+Z1.*r2.*w2+Z1.*r3.*w3.*2.0+r1.*t1.*w3.*2.0+r2.*t2.*w3.*2.0+... - r3.*t3.*w3.*2.0+X1.*r2.*t7.*t12+X1.*r2.*t9.*t10-Y1.*r1.*t7.*t12-Y1.*r1.*t9.*t10+... - X1.*r1.*t12.*w3.*2.0-X1.*r3.*t12.*w1+Y1.*r2.*t12.*w3.*2.0-Y1.*r3.*t12.*w2-... - Z1.*r1.*t12.*w1-Z1.*r2.*t12.*w2+X1.*r2.*t7.*t10.*t11-Y1.*r1.*t7.*t10.*t11-... - X1.*r3.*t12.*w2.*w3+Y1.*r3.*t12.*w1.*w3+Z1.*r1.*t12.*w2.*w3-Z1.*r2.*t12.*w1.*w3+... - Y1.*r3.*t10.*t11.*w1.*w3+Z1.*r1.*t10.*t11.*w2.*w3-Z1.*r2.*t10.*t11.*w1.*w3-... - X1.*r1.*t6.*t10.*t11.*w3-X1.*r1.*t7.*t10.*t11.*w3+X1.*r3.*t7.*t10.*t11.*w1-... - Y1.*r2.*t5.*t10.*t11.*w3-Y1.*r2.*t7.*t10.*t11.*w3+Y1.*r3.*t7.*t10.*t11.*w2+... - Z1.*r1.*t7.*t10.*t11.*w1+Z1.*r2.*t7.*t10.*t11.*w2-Z1.*r3.*t5.*t10.*t11.*w3-... - Z1.*r3.*t6.*t10.*t11.*w3-X1.*r3.*t10.*t11.*w2.*w3+X1.*r2.*t10.*t11.*w1.*w2.*w3+... - Y1.*r1.*t10.*t11.*w1.*w2.*w3)-t26.*t65.*t93.*w3.*2.0-t14.*t93.*t101.*(t18.*(Z1.*(t46-... - t113+t114+t115-t13.*t14.*w2)-Y1.*t198+X1.*(t49+t51+t52+t118-t7.*t10.*t25)).*2.0+... - t23.*(X1.*(-t97+t112+t116+t117-t13.*t14.*w1)+Y1.*(-t46+t113+t114+t115-t13.*t14.*w2)-... - Z1.*t195).*2.0+t15.*(t204+Z1.*(t97-t112+t116+t117-t13.*t14.*w1)-... - X1.*(t201+t202-t13.*t14.*w3.*2.0)).*2.0).*(1.0./2.0),t14.*t65.*(X1.*s3.*w1+... - Y1.*s3.*w2+Z1.*s1.*w1+Z1.*s2.*w2+Z1.*s3.*w3.*2.0+s1.*t1.*w3.*2.0+s2.*t2.*w3.*2.0+... - s3.*t3.*w3.*2.0+X1.*s2.*t7.*t12+X1.*s2.*t9.*t10-Y1.*s1.*t7.*t12-Y1.*s1.*t9.*t10+... - X1.*s1.*t12.*w3.*2.0-X1.*s3.*t12.*w1+Y1.*s2.*t12.*w3.*2.0-... - Y1.*s3.*t12.*w2-Z1.*s1.*t12.*w1-Z1.*s2.*t12.*w2+X1.*s2.*t7.*t10.*t11-... - Y1.*s1.*t7.*t10.*t11-X1.*s3.*t12.*w2.*w3+Y1.*s3.*t12.*w1.*w3+Z1.*s1.*t12.*w2.*w3-... - Z1.*s2.*t12.*w1.*w3-X1.*s3.*t10.*t11.*w2.*w3+Y1.*s3.*t10.*t11.*w1.*w3+... - Z1.*s1.*t10.*t11.*w2.*w3-Z1.*s2.*t10.*t11.*w1.*w3-X1.*s1.*t6.*t10.*t11.*w3-... - X1.*s1.*t7.*t10.*t11.*w3+X1.*s3.*t7.*t10.*t11.*w1-Y1.*s2.*t5.*t10.*t11.*w3-... - Y1.*s2.*t7.*t10.*t11.*w3+Y1.*s3.*t7.*t10.*t11.*w2+Z1.*s1.*t7.*t10.*t11.*w1+... - Z1.*s2.*t7.*t10.*t11.*w2-Z1.*s3.*t5.*t10.*t11.*w3-Z1.*s3.*t6.*t10.*t11.*w3+... - X1.*s2.*t10.*t11.*w1.*w2.*w3+Y1.*s1.*t10.*t11.*w1.*w2.*w3)-t26.*t65.*t167.*w3.*2.0-... - t14.*t101.*t167.*(t18.*(Z1.*(t46-t113+t114+t115-t138)-Y1.*t198+... - X1.*(t49+t51+t52+t118-t199)).*2.0+t23.*(X1.*(-t97+t112+t116+t117-... - t192)+Y1.*(-t46+t113+t114+t115-t138)-Z1.*t195).*2.0+t15.*(t204+Z1.*(t97-t112+... - t116+t117-t192)-X1.*(-t200+t201+t202)).*2.0).*(1.0./2.0),r1.*t65-... - t14.*t93.*t101.*t208.*(1.0./2.0),s1.*t65-t14.*t101.*t167.*t208.*(1.0./2.0),r2.*t65-... - t14.*t93.*t101.*t212.*(1.0./2.0),s2.*t65-t14.*t101.*t167.*t212.*(1.0./2.0),r3.*t65-... - t14.*t93.*t101.*t216.*(1.0./2.0),s3.*t65-t14.*t101.*t167.*t216.*(1.0./2.0)],[2,6]); diff --git a/MLPnP/optim_MLPnP_GN.m b/MLPnP/optim_MLPnP_GN.m index ffc2e77..4e11c24 100644 --- a/MLPnP/optim_MLPnP_GN.m +++ b/MLPnP/optim_MLPnP_GN.m @@ -16,68 +16,43 @@ % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. % 28.06.2016 Steffen Urban +% 18.08.2023 Shi Shenglei + +function [Rout, tout] = optim_MLPnP_GN(R, t, points3D, rnull, snull, P, optimFlags) + nrPts = size(points3D,2); + + % optim params + epsParam = optimFlags.epsP; + epsFunc = optimFlags.epsF; + + % iteration params + cnt = 0; + stop = false; + invKll = P; + while cnt < optimFlags.maxit && stop == 0 + [r, J] = residualsAndJacobian(R, t, rnull, snull, points3D); + % design matrix + N = J.'*invKll*J; + % System matrix + g = J.'*invKll*r; + + dx = pinv(N)*g; + if (max(abs(dx)) > 20 || min(abs(dx)) > 1) + break; + end -function [Tout, statistic] = optim_MLPnP_GN(Tinit, points3D, ... - rnull, snull, P, optimFlags) - -% homogeneous to minimal -x = [Rodrigues2(Tinit(1:3,1:3))', Tinit(1:3,4)']'; - -nrL = size(rnull,2); - -% redundancy -redundanz = 2*nrL - length(x); -% optim params -epsParam = optimFlags.epsP; -epsFunc = optimFlags.epsF; - -% iteration params -cnt = 0; -stop = false; -invKll = P; -while cnt < optimFlags.maxit && stop == 0 - [r, J] = residualsAndJacobian(x, rnull, snull, points3D); - % design matrix - N = J.'*invKll*J; - % System matrix - g = J.'*invKll*r; - - dx = pinv(N)*g; - if (max(abs(dx)) > 20 || min(abs(dx)) > 1) - break; - end - dl = J*dx(1:end); - - if max(abs(dl)) < epsFunc || max(abs(dx(1:end))) < epsParam - x = x-dx; - break; - else % update parameter vector - x = x-dx; - end - cnt = cnt+1; -end % while loop - - -% minimal to homogeneous -Tout = [Rodrigues2(x(1:3)) x(4:6)]; - -% empirical variance factor -resV = r.'*invKll*r; -if redundanz > 0 - if redundanz < nrL - s0 = 1; - else - s0 = resV / redundanz; - end -else - s0 = NaN; + dR = Rodrigues2(-dx(1:3)); + R = dR * R; + t = dR * t - dx(4:6); + + dl = J*dx(1:end); + if max(abs(dl)) < epsFunc || max(abs(dx(1:end))) < epsParam + break; + end + cnt = cnt+1; + end % while loop + + Rout = R; + tout = t; end -% variance-covariance matrix -Qxx = pinv(N); -% cofactor matrix of "adjusted observations" -Qldld = J*Qxx*J'; - -statistic = {resV, r, J, Qxx, s0, Qldld, sqrt(s0.*diag(Qxx))}; -end - diff --git a/MLPnP/residualsAndJacobian.m b/MLPnP/residualsAndJacobian.m index eb39893..bf24aee 100644 --- a/MLPnP/residualsAndJacobian.m +++ b/MLPnP/residualsAndJacobian.m @@ -16,24 +16,18 @@ % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. % 28.06.2016 Steffen Urban -function [err,J] = residualsAndJacobian(x, r, s, points3D) +% 18.08.2023 Shi Shenglei +function [err,J] = residualsAndJacobian(R, t, r, s, points3D) + nrPts = size(points3D,2); + err = zeros(2*nrPts,1); + J = zeros(2*nrPts,6); -nrPts = size(points3D,2); + res1 = R*points3D+repmat(t,1,nrPts); + for i=1:nrPts + err(2*i-1,1) = r(:,i)'*res1(:,i); + err(2*i,1) = s(:,i)'*res1(:,i); -err = zeros(2*nrPts,1); -J = zeros(2*nrPts,6); -R = Rodrigues2(x(1:3)); -t = x(4:6); - -res1 = R*points3D+repmat(t,1,nrPts); -normres = normc(res1(1:3,:)); - -for i=1:size(r,2) - err(2*i-1,1) = r(:,i)'*normres(:,i); - err(2*i,1) = s(:,i)'*normres(:,i); - J(2*i-1:2*i,1:6) = jacobians_Rodrigues(points3D(1,i),points3D(2,i),points3D(3,i),... - r(1,i),r(2,i),r(3,i),s(1,i),s(2,i),s(3,i),x(4),x(5),x(6),x(1),x(2),x(3)); + J(2*i-1,:) = [-r(:,i)'*skew(res1(:,i)) r(:,i)']; + J(2*i,:) = [-s(:,i)'*skew(res1(:,i)) s(:,i)']; + end end - -end - diff --git a/MLPnP/skew.m b/MLPnP/skew.m new file mode 100644 index 0000000..2eeb23d --- /dev/null +++ b/MLPnP/skew.m @@ -0,0 +1,24 @@ +% Steffen Urban email: urbste@googlemail.com +% Copyright (C) 2016 Steffen Urban +% +% This program is free software; you can redistribute it and/or modify +% it under the terms of the GNU General Public License as published by +% the Free Software Foundation; either version 2 of the License, or +% (at your option) any later version. +% +% This program is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License along +% with this program; if not, write to the Free Software Foundation, Inc., +% 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +% 28.06.2016 Steffen Urban + +function [wv] = skew(v) + wv = [ 0 -v(3) v(2); + v(3) 0 -v(1); + -v(2) v(1) 0 ]; +end diff --git a/main_ordinary_3d.m b/main_ordinary_3d.m index 1756f97..8240cfd 100644 --- a/main_ordinary_3d.m +++ b/main_ordinary_3d.m @@ -33,68 +33,68 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 28.06.2016 Steffen Urban -clear; clc; + +clear all; clc; IniToolbox; % experimental parameters +% sigma nl= [1,2,3,4,5,6,7,8,9,10]; nlsamples = [0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]; %percentatge of samples for each sigma npts= [10:10:200]; num = 200; % compared methods -A= zeros(size(npts)); -B= zeros(num,1); - % if you want to use UPnP you have to download and install OpenGV % also if you want to reproduce the runtime shown in the MLPnP paper -% yout have to install the OpenGV fork with MLPnP -name = {'MLPnPWithCov','MLPnP','LHM', 'EPnP+GN', 'RPnP', 'DLS', 'PPnP', 'ASPnP', 'SDP','OPnP', 'EPPnP', 'CEPPnP'}; -f = {@MLPNP_with_COV, @MLPNP_without_COV,@LHM, @EPnP_GN, @RPnP, @robust_dls_pnp, @PPnP, @ASPnP, @GOP, @OPnP, @EPPnP, @CEPPnP}; +% yout have to install the OpenGV fork with MLPnP + +name = {'MLPnPWithCov','MLPnP','LHM', 'EPnP+GN', 'RPnP', 'DLS', 'PPnP', 'ASPnP', 'SDP', 'OPnP', 'EPPnP', 'CEPPnP'}; +f = {@MLPNP_with_COV, @MLPNP_without_COV, @LHM, @EPnP_GN, @RPnP, @robust_dls_pnp, @PPnP, @ASPnP, @GOP, @OPnP, @EPPnP, @CEPPnP}; marker = { 'x', 'd', 's', 'd', '^', '*', '<', 'v', '>','o','+','<'}; color = {'g', 'g', 'c', [1,0.5,0],'m', [1,0.5,1], 'b', 'y', 'r','k','k',[1,0.5,0.5]}; markerfacecolor = {'g','b','c',[1,0.5,0],'m', [1,0.5,1], 'b', 'y', 'r','k','n',[0,0.5,0.5]}; - -method_list= struct('name', name, 'f', f, 'mean_r', A, 'mean_t', A,... - 'med_r', A, 'med_t', A, 'std_r', A, 'std_t', A, 'r', B, 't', B,... +method_list= struct('name', name, 'f', f, 'mean_r', zeros(size(npts)), 'mean_t', zeros(size(npts)),... + 'med_r', zeros(size(npts)), 'med_t', zeros(size(npts)), 'std_r', zeros(size(npts)), 'std_t', zeros(size(npts)),... + 'mean_e', zeros(size(npts)),... 'marker', marker, 'color', color, 'markerfacecolor', markerfacecolor); - - - meanR = zeros(length(npts),length(method_list)+1); medianR = zeros(length(npts),length(method_list)+1); meanT = zeros(length(npts),length(method_list)+1); medianT = zeros(length(npts),length(method_list)+1); medianTime = zeros(length(npts),length(method_list)+1); +meanE = zeros(length(npts),length(method_list)+1); + +% camera's parameters +f= 800; +K = [f 0 0; + 0 f 0; + 0 0 1]; + % experiments -for i= 1:length(npts) +for i = 1:length(npts) - npt= npts(i); - fprintf('npt = %d (num sg = %d ): ', npt, length(nl)); - - - for k= 1:length(method_list) - method_list(k).c = zeros(1,num); - method_list(k).e = zeros(1,num); - method_list(k).r = zeros(1,num); - method_list(k).t = zeros(1,num); - end + npt= npts(i); + fprintf('npt = %d (num sg = %d ): ', npt, length(nl)); + + for k= 1:length(method_list) + method_list(k).c = zeros(1,num); + method_list(k).e = zeros(1,num); + method_list(k).r = zeros(1,num); + method_list(k).t = zeros(1,num); + end index_fail = cell(1,length(name)); - for j= 1:num - % camera's parameters - width= 640; - height= 480; - f= 800; - K = [f 0 0 - 0 f 0 - 0 0 1]; + for j= 1:num % generate 3d coordinates in camera space Xc= [xrand(1,npt,[-2 2]); xrand(1,npt,[-2 2]); xrand(1,npt,[4 8])]; + % cw t= mean(Xc,2); R= rodrigues(randn(3,1)); + + % world 3d points XXw= inv(R)*(Xc-repmat(t,1,npt)); % projection @@ -107,17 +107,16 @@ sigma = nl(idnl); nls(id:id+nnl(idnl)-1) = sigma .* ones(1,nnl(idnl)); id = id+nnl(idnl); - end randomvals = randn(2,npt); xxn= xx+randomvals.*[nls;nls]; - homx = [xxn/f; ones(1,size(xxn,2))]; + homx = [xxn/f; ones(1,npt)]; v = normc(homx); Cu = zeros(2,2,npt); Evv = zeros(3,3,npt); - cov = zeros(9,size(Cu,3)); + cov = zeros(9,npt); for id = 1:npt Cu(:,:,id) = diag([nls(id)^2 nls(id)^2]); cov_proj = K\diag([nls(id)^2 nls(id)^2 0])/K'; @@ -125,9 +124,7 @@ Evv(:,:,id) = J*cov_proj*J'; cov(:,id) = reshape(Evv(:,:,id),9,1); end - - - + % pose estimation for k= 1:length(method_list) try @@ -139,30 +136,29 @@ tcost = toc; elseif strcmp(method_list(k).name, 'MLPnP') || strcmp(method_list(k).name, 'MLPnPWithCov') tic; - [R1,t1]= method_list(k).f(XXw,v,cov); + [R1,t1]= method_list(k).f(XXw, v, cov); tcost = toc; else tic; - [R1,t1]= method_list(k).f(XXw,xxn/f); + [R1,t1]= method_list(k).f(XXw, xxn/f); tcost = toc; end catch disp(['The solver - ',method_list(k).name,' - encounters internal errors!!!']); - %index_fail = [index_fail, j]; index_fail{k} = [index_fail{k}, j]; - break; + continue; end %no solution if size(t1,2) < 1 disp(['The solver - ',method_list(k).name,' - returns no solution!!!']); - %index_fail = [index_fail, j]; index_fail{k} = [index_fail{k}, j]; - break; + continue; elseif (sum(sum(sum(imag(R1).^2))>0) == size(R1,3) || sum(sum(imag(t1(:,:,1)).^2)>0) == size(t1,2)) index_fail{k} = [index_fail{k}, j]; continue; end + %choose the solution with smallest error error = inf; for jjj = 1:size(R1,3) @@ -187,69 +183,68 @@ % save result for k= 1:length(method_list) - %results without deleting solutions - tmethod_list = method_list(k); - method_list(k).c(index_fail{k}) = []; - method_list(k).e(index_fail{k}) = []; - method_list(k).r(index_fail{k}) = []; - method_list(k).t(index_fail{k}) = []; - - % computational cost should be computed in a separated procedure as - % in main_time.m - - method_list(k).pfail(i) = 100 * numel(index_fail{k})/num; - - method_list(k).mean_c(i)= mean(method_list(k).c); - method_list(k).mean_e(i)= mean(method_list(k).e); - method_list(k).med_c(i)= median(method_list(k).c); - method_list(k).med_e(i)= median(method_list(k).e); - method_list(k).std_c(i)= std(method_list(k).c); - method_list(k).std_e(i)= std(method_list(k).e); - - method_list(k).mean_r(i)= mean(method_list(k).r); - method_list(k).mean_t(i)= mean(method_list(k).t); - method_list(k).med_r(i)= median(method_list(k).r); - method_list(k).med_t(i)= median(method_list(k).t); - method_list(k).std_r(i)= std(method_list(k).r); - method_list(k).std_t(i)= std(method_list(k).t); - - meanR (i,1) = npts(i); - meanT (i,1) = npts(i); - medianR (i,1) = npts(i); - medianT (i,1) = npts(i); - medianTime(i,1) = npts(i); - - meanR(i,k+1) = method_list(k).mean_r(i); - meanT(i,k+1) = method_list(k).mean_t(i); - medianR(i,k+1) = method_list(k).med_r(i); - medianT(i,k+1) = method_list(k).med_t(i); - - medianTime(i,k+1) = method_list(k).med_c(i); - - %results deleting solutions where not all the methods finds one - tmethod_list.c(unique([index_fail{:}])) = []; - tmethod_list.e(unique([index_fail{:}])) = []; - tmethod_list.r(unique([index_fail{:}])) = []; - tmethod_list.t(unique([index_fail{:}])) = []; - - method_list(k).deleted_mean_c(i)= mean(tmethod_list.c); - method_list(k).deleted_mean_e(i)= mean(tmethod_list.e); - method_list(k).deleted_med_c(i)= median(tmethod_list.c); - method_list(k).deleted_med_e(i)= median(tmethod_list.e); - method_list(k).deleted_std_c(i)= std(tmethod_list.c); - method_list(k).deleted_std_e(i)= std(tmethod_list.e); - - method_list(k).deleted_mean_r(i)= mean(tmethod_list.r); - method_list(k).deleted_mean_t(i)= mean(tmethod_list.t); - method_list(k).deleted_med_r(i)= median(tmethod_list.r); - method_list(k).deleted_med_t(i)= median(tmethod_list.t); - method_list(k).deleted_std_r(i)= std(tmethod_list.r); - method_list(k).deleted_std_t(i)= std(tmethod_list.t); - - + %results without deleting solutions + tmethod_list = method_list(k); + method_list(k).c(index_fail{k}) = []; + method_list(k).e(index_fail{k}) = []; + method_list(k).r(index_fail{k}) = []; + method_list(k).t(index_fail{k}) = []; + + % computational cost should be computed in a separated procedure as + % in main_time.m + + method_list(k).pfail(i) = 100 * numel(index_fail{k})/num; + + method_list(k).mean_c(i)= mean(method_list(k).c); + method_list(k).mean_e(i)= mean(method_list(k).e); + method_list(k).med_c(i)= median(method_list(k).c); + method_list(k).med_e(i)= median(method_list(k).e); + method_list(k).std_c(i)= std(method_list(k).c); + method_list(k).std_e(i)= std(method_list(k).e); + + method_list(k).mean_r(i)= mean(method_list(k).r); + method_list(k).mean_t(i)= mean(method_list(k).t); + method_list(k).med_r(i)= median(method_list(k).r); + method_list(k).med_t(i)= median(method_list(k).t); + method_list(k).std_r(i)= std(method_list(k).r); + method_list(k).std_t(i)= std(method_list(k).t); + + meanR (i,1) = npt; + meanT (i,1) = npt; + medianR (i,1) = npt; + medianT (i,1) = npt; + medianTime(i,1) = npt; + meanE (i,1) = npt; + + meanR(i,k+1) = method_list(k).mean_r(i); + meanT(i,k+1) = method_list(k).mean_t(i); + medianR(i,k+1) = method_list(k).med_r(i); + medianT(i,k+1) = method_list(k).med_t(i); + medianTime(i,k+1) = method_list(k).med_c(i); + meanE(i,k+1) = method_list(k).mean_e(i); + + %results deleting solutions where not all the methods finds one + tmethod_list.c(unique([index_fail{:}])) = []; + tmethod_list.e(unique([index_fail{:}])) = []; + tmethod_list.r(unique([index_fail{:}])) = []; + tmethod_list.t(unique([index_fail{:}])) = []; + + method_list(k).deleted_mean_c(i)= mean(tmethod_list.c); + method_list(k).deleted_mean_e(i)= mean(tmethod_list.e); + method_list(k).deleted_med_c(i)= median(tmethod_list.c); + method_list(k).deleted_med_e(i)= median(tmethod_list.e); + method_list(k).deleted_std_c(i)= std(tmethod_list.c); + method_list(k).deleted_std_e(i)= std(tmethod_list.e); + + method_list(k).deleted_mean_r(i)= mean(tmethod_list.r); + method_list(k).deleted_mean_t(i)= mean(tmethod_list.t); + method_list(k).deleted_med_r(i)= median(tmethod_list.r); + method_list(k).deleted_med_t(i)= median(tmethod_list.t); + method_list(k).deleted_std_r(i)= std(tmethod_list.r); + method_list(k).deleted_std_t(i)= std(tmethod_list.t); end end -save ordinary3Dresults method_list npts; +%save ordinary3Dresults method_list npts; -plotOrdinary3D; \ No newline at end of file +plotOrdinary3D; diff --git a/main_planar.m b/main_planar.m index db157f1..19b878c 100644 --- a/main_planar.m +++ b/main_planar.m @@ -33,77 +33,69 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 28.06.2016 Steffen Urban - -clear; clc; +clear all; clc; IniToolbox; % experimental parameters nl= [1,2,3,4,5,6,7,8,9,10]; -%nl= 5*ones(1,10); nlsamples = [0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]; %percentatge of samples for each sigma npts= [10:10:200]; - num = 200; - -% compared methods -A= zeros(size(npts)); -B= zeros(num,1); - % if you want to use UPnP you have to download and install OpenGV % also if you want to reproduce the runtime shown in the MLPnP paper % yout have to install the OpenGV fork with MLPnP -name= {'MLPnPWithCov','MLPnP','LHM', 'EPnP','RPnP', 'DLS','PPnP', 'ASPnP','OPnP','EPPnP','CEPPnP'}; -f= {@MLPNP_with_COV, @MLPNP_without_COV,@LHM, @EPnP_planar, @RPnP, @robust_dls_pnp, @PPnP, @ASPnP,@OPnP, @EPPnP_planar, @CEPPnP_planar}; -marker= { 'x', 'd', 'x', 's', 'd', '^', '*', '<', '>','o','+'}; -color= {'g', 'g','c', 'r', [1,0.5,0],'m', [1,0.5,1], 'b', 'r','k',[0,0.5,0.5]}; -markerfacecolor= {'g','g','c','g',[1,0.5,0],'m', [1,0.5,1], 'b', 'r','k',[0,0.5,0.5]}; - -method_list= struct('name', name, 'f', f, 'mean_r', A, 'mean_t', A,... - 'med_r', A, 'med_t', A, 'std_r', A, 'std_t', A, 'r', B, 't', B,... - 'marker', marker, 'color', color, 'markerfacecolor', markerfacecolor); +name = {'MLPnPWithCov','MLPnP','LHM', 'EPnP+GN', 'RPnP', 'DLS', 'PPnP', 'ASPnP', 'SDP', 'OPnP', 'EPPnP', 'CEPPnP'}; +f = {@MLPNP_with_COV, @MLPNP_without_COV, @LHM, @EPnP_GN, @RPnP, @robust_dls_pnp, @PPnP, @ASPnP, @GOP, @OPnP, @EPPnP, @CEPPnP}; +marker = { 'x', 'd', 's', 'd', '^', '*', '<', 'v', '>','o','+','<'}; +color = {'g', 'g', 'c', [1,0.5,0],'m', [1,0.5,1], 'b', 'y', 'r','k','k',[1,0.5,0.5]}; +markerfacecolor = {'g','b','c',[1,0.5,0],'m', [1,0.5,1], 'b', 'y', 'r','k','n',[0,0.5,0.5]}; + +method_list= struct('name', name, 'f', f, 'mean_r', zeros(size(npts)), 'mean_t', zeros(size(npts)),... + 'med_r', zeros(size(npts)), 'med_t', zeros(size(npts)), 'std_r', zeros(size(npts)), 'std_t', zeros(size(npts)),... + 'mean_e', zeros(size(npts)),... + 'marker', marker, 'color', color, 'markerfacecolor', markerfacecolor); meanR = zeros(length(npts),length(method_list)+1); medianR = zeros(length(npts),length(method_list)+1); meanT = zeros(length(npts),length(method_list)+1); medianT = zeros(length(npts),length(method_list)+1); medianTime = zeros(length(npts),length(method_list)+1); +meanE = zeros(length(npts),length(method_list)+1); + +% camera's parameters +f= 800; +K = [f 0 0; + 0 f 0; + 0 0 1]; + % experiments for i= 1:length(npts) - + npt= npts(i); fprintf('npt = %d (num sg = %d ): ', npt, length(nl)); - - for k= 1:length(method_list) + for k= 1:length(method_list) method_list(k).c = zeros(1,num); method_list(k).e = zeros(1,num); method_list(k).r = zeros(1,num); method_list(k).t = zeros(1,num); end - %index_fail = []; index_fail = cell(1,length(name)); for j= 1:num - - % camera's parameters - width= 640; - height= 480; - f= 800; - K = [f 0 0 - 0 f 0 - 0 0 1]; - % generate 3d coordinates in camera space + % generate 3d coordinates in world space XXw= [xrand(2,npt,[-2 2]); zeros(1,npt)]; - R= rodrigues(randn(3,1)); + + R= rodrigues(randn(3,1)); % cw t= [rand-0.5;rand-0.5;rand*4+4]; Xc= R*XXw+repmat(t,1,npt); + % projection xx= [Xc(1,:)./Xc(3,:); Xc(2,:)./Xc(3,:)]*f; - randomvals = randn(2,npt); nnl = round(npt * nlsamples); nls = zeros(1,npt); @@ -112,9 +104,9 @@ sigma = nl(idnl); nls(id:id+nnl(idnl)-1) = sigma .* ones(1,nnl(idnl)); id = id+nnl(idnl); - end + randomvals = randn(2,npt); xxn= xx+randomvals.*[nls;nls]; homx = [xxn/f; ones(1,size(xxn,2))]; v = normc(homx); @@ -132,40 +124,38 @@ % pose estimation for k= 1:length(method_list) -% try - if strcmp(method_list(k).name, 'CEPPnP') + try + if strcmp(method_list(k).name, 'CEPPnP') tic; mXXw = XXw - repmat(mean(XXw,2),1,size(XXw,2)); [R1,t1]= method_list(k).f(mXXw,xxn/f,Cu); t1 = t1 - R1 * mean(XXw,2); - tcost = toc; - elseif strcmp(method_list(k).name, 'MLPnP') || ... - strcmp(method_list(k).name, 'MLPnPWithCov') + tcost = toc; + elseif strcmp(method_list(k).name, 'MLPnP') || strcmp(method_list(k).name, 'MLPnPWithCov') tic; - [R1,t1]= method_list(k).f(XXw,v,cov); + [R1,t1]= method_list(k).f(XXw, v, cov); tcost = toc; else tic; - [R1,t1]= method_list(k).f(XXw,xxn/f); + [R1,t1]= method_list(k).f(XXw, xxn/f); tcost = toc; - end -% catch -% disp(['The solver - ',method_list(k).name,' - encounters internal errors!!!']); -% %index_fail = [index_fail, j]; -% index_fail{k} = [index_fail{k}, j]; -% break; -% end + end + catch + disp(['The solver - ',method_list(k).name,' - encounters internal errors!!!']); + index_fail{k} = [index_fail{k}, j]; + continue; + end %no solution if size(t1,2) < 1 disp(['The solver - ',method_list(k).name,' - returns no solution!!!']); - %index_fail = [index_fail, j]; index_fail{k} = [index_fail{k}, j]; - break; + continue; elseif (sum(sum(sum(imag(R1).^2))>0) == size(R1,3) || sum(sum(imag(t1(:,:,1)).^2)>0) == size(t1,2)) index_fail{k} = [index_fail{k}, j]; continue; end + %choose the solution with smallest error error = inf; for jjj = 1:size(R1,3) @@ -191,64 +181,67 @@ % save result for k= 1:length(method_list) %results without deleting solutions - tmethod_list = method_list(k); - method_list(k).c(index_fail{k}) = []; - method_list(k).e(index_fail{k}) = []; - method_list(k).r(index_fail{k}) = []; - method_list(k).t(index_fail{k}) = []; - - % computational cost should be computed in a separated procedure as - % in main_time.m - - method_list(k).pfail(i) = 100 * numel(index_fail{k})/num; - - method_list(k).mean_c(i)= mean(method_list(k).c); - method_list(k).mean_e(i)= mean(method_list(k).e); - method_list(k).med_c(i)= median(method_list(k).c); - method_list(k).med_e(i)= median(method_list(k).e); - method_list(k).std_c(i)= std(method_list(k).c); - method_list(k).std_e(i)= std(method_list(k).e); - - method_list(k).mean_r(i)= mean(method_list(k).r); - method_list(k).mean_t(i)= mean(method_list(k).t); - method_list(k).med_r(i)= median(method_list(k).r); - method_list(k).med_t(i)= median(method_list(k).t); - method_list(k).std_r(i)= std(method_list(k).r); - method_list(k).std_t(i)= std(method_list(k).t); - - meanR (i,1) = npts(i); - meanT (i,1) = npts(i); - medianR (i,1) = npts(i); - medianT (i,1) = npts(i); - medianTime(i,1) = npts(i); - - meanR(i,k+1) = method_list(k).mean_r(i); - meanT(i,k+1) = method_list(k).mean_t(i); - medianR(i,k+1) = method_list(k).med_r(i); - medianT(i,k+1) = method_list(k).med_t(i); - - medianTime(i,k+1) = method_list(k).med_c(i); - %results deleting solutions where not all the methods finds one - tmethod_list.c(unique([index_fail{:}])) = []; - tmethod_list.e(unique([index_fail{:}])) = []; - tmethod_list.r(unique([index_fail{:}])) = []; - tmethod_list.t(unique([index_fail{:}])) = []; - - method_list(k).deleted_mean_c(i)= mean(tmethod_list.c); - method_list(k).deleted_mean_e(i)= mean(tmethod_list.e); - method_list(k).deleted_med_c(i)= median(tmethod_list.c); - method_list(k).deleted_med_e(i)= median(tmethod_list.e); - method_list(k).deleted_std_c(i)= std(tmethod_list.c); - method_list(k).deleted_std_e(i)= std(tmethod_list.e); - - method_list(k).deleted_mean_r(i)= mean(tmethod_list.r); - method_list(k).deleted_mean_t(i)= mean(tmethod_list.t); - method_list(k).deleted_med_r(i)= median(tmethod_list.r); - method_list(k).deleted_med_t(i)= median(tmethod_list.t); - method_list(k).deleted_std_r(i)= std(tmethod_list.r); - method_list(k).deleted_std_t(i)= std(tmethod_list.t); + tmethod_list = method_list(k); + method_list(k).c(index_fail{k}) = []; + method_list(k).e(index_fail{k}) = []; + method_list(k).r(index_fail{k}) = []; + method_list(k).t(index_fail{k}) = []; + + % computational cost should be computed in a separated procedure as + % in main_time.m + + method_list(k).pfail(i) = 100 * numel(index_fail{k})/num; + + method_list(k).mean_c(i)= mean(method_list(k).c); + method_list(k).mean_e(i)= mean(method_list(k).e); + method_list(k).med_c(i)= median(method_list(k).c); + method_list(k).med_e(i)= median(method_list(k).e); + method_list(k).std_c(i)= std(method_list(k).c); + method_list(k).std_e(i)= std(method_list(k).e); + + method_list(k).mean_r(i)= mean(method_list(k).r); + method_list(k).mean_t(i)= mean(method_list(k).t); + method_list(k).med_r(i)= median(method_list(k).r); + method_list(k).med_t(i)= median(method_list(k).t); + method_list(k).std_r(i)= std(method_list(k).r); + method_list(k).std_t(i)= std(method_list(k).t); + + meanR (i,1) = npt; + meanT (i,1) = npt; + medianR (i,1) = npt; + medianT (i,1) = npt; + medianTime(i,1) = npt; + meanE (i,1) = npt; + + meanR(i,k+1) = method_list(k).mean_r(i); + meanT(i,k+1) = method_list(k).mean_t(i); + medianR(i,k+1) = method_list(k).med_r(i); + medianT(i,k+1) = method_list(k).med_t(i); + medianTime(i,k+1) = method_list(k).med_c(i); + meanE(i,k+1) = method_list(k).mean_e(i); + + %results deleting solutions where not all the methods finds one + tmethod_list.c(unique([index_fail{:}])) = []; + tmethod_list.e(unique([index_fail{:}])) = []; + tmethod_list.r(unique([index_fail{:}])) = []; + tmethod_list.t(unique([index_fail{:}])) = []; + + method_list(k).deleted_mean_c(i)= mean(tmethod_list.c); + method_list(k).deleted_mean_e(i)= mean(tmethod_list.e); + method_list(k).deleted_med_c(i)= median(tmethod_list.c); + method_list(k).deleted_med_e(i)= median(tmethod_list.e); + method_list(k).deleted_std_c(i)= std(tmethod_list.c); + method_list(k).deleted_std_e(i)= std(tmethod_list.e); + + method_list(k).deleted_mean_r(i)= mean(tmethod_list.r); + method_list(k).deleted_mean_t(i)= mean(tmethod_list.t); + method_list(k).deleted_med_r(i)= median(tmethod_list.r); + method_list(k).deleted_med_t(i)= median(tmethod_list.t); + method_list(k).deleted_std_r(i)= std(tmethod_list.r); + method_list(k).deleted_std_t(i)= std(tmethod_list.t); end end -save planar3Dresults method_list npts; +%save planar3Dresults method_list npts; + plotPlanar3D; diff --git a/plot_funcs/plotOrdinary3D.m b/plot_funcs/plotOrdinary3D.m index ff36282..167dfe6 100644 --- a/plot_funcs/plotOrdinary3D.m +++ b/plot_funcs/plotOrdinary3D.m @@ -1,6 +1,6 @@ addpath plot_funcs; -load ordinary3Dresults +%load ordinary3Dresults close all; yrange= [0 2]; @@ -51,4 +51,4 @@ figure('color','w','position',[w*i,100+h,w,h]);i=i+1; xdrawgraph(npts,yrange,method_list,'deleted_med_c','Median Cost',... - 'Number of Points','Cost (ms)'); \ No newline at end of file + 'Number of Points','Cost (ms)'); diff --git a/plot_funcs/plotPlanar3D.m b/plot_funcs/plotPlanar3D.m index 6e57836..a686c7b 100644 --- a/plot_funcs/plotPlanar3D.m +++ b/plot_funcs/plotPlanar3D.m @@ -1,6 +1,6 @@ addpath plot_funcs; -load planar3Dresults +%load planar3Dresults close all; yrange= [0 2]; @@ -51,4 +51,4 @@ figure('color','w','position',[w*i,100+h,w,h]);i=i+1; xdrawgraph(npts,yrange,method_list,'deleted_med_c','Median Cost',... - 'Number of Points','Cost (ms)'); \ No newline at end of file + 'Number of Points','Cost (ms)'); diff --git a/rpnp/code3/func/xdrawgraph.m b/rpnp/code3/func/xdrawgraph.m index 6aa8bf7..41244bb 100644 --- a/rpnp/code3/func/xdrawgraph.m +++ b/rpnp/code3/func/xdrawgraph.m @@ -1,28 +1,21 @@ -function xdrawgraph(xs,yrange,method_list,field,ti,lx,ly,legendposition) -%the legend is at upper right in default -if nargin < 8 - legendposition = 1; -end +function xdrawgraph(xs,yrange,method_list,field,ti,lx,ly) + box('on'); + hold('all'); -box('on'); -hold('all'); + for i= 1:length(method_list) + plot(xs,method_list(i).(field),'marker',method_list(i).marker,... + 'color',method_list(i).color,... + 'markerfacecolor',method_list(i).markerfacecolor,... + 'displayname',method_list(i).name, ... + 'LineWidth',2,'MarkerSize',8); + % ); + end + ylim(yrange); + xlim(xs([1 end])); + set(gca,'xtick',xs); -p= zeros(size(method_list)); -for i= 1:length(method_list) - p(i)= plot(xs,method_list(i).(field),'marker',method_list(i).marker,... - 'color',method_list(i).color,... - 'markerfacecolor',method_list(i).markerfacecolor,... - 'displayname',method_list(i).name, ... - 'LineWidth',2,'MarkerSize',8); -% ); + title(ti,'FontSize',12,'FontName','Arial'); + xlabel(lx,'FontSize',11); + ylabel(ly,'FontSize',11); + legend; end -ylim(yrange); -xlim(xs([1 end])); -set(gca,'xtick',xs); - -title(ti,'FontSize',12,'FontName','Arial'); -xlabel(lx,'FontSize',11); -ylabel(ly,'FontSize',11); -legend(p,legendposition); - -return From 36dbe3ac9709194625da0a200dc6b58dda292d8b Mon Sep 17 00:00:00 2001 From: shshlei Date: Fri, 18 Aug 2023 14:14:52 +0800 Subject: [PATCH 2/2] Using ExpMap --- MLPnP/MLPNP_with_COV.m | 4 +--- MLPnP/MLPNP_without_COV.m | 4 +--- MLPnP/MLPnP.m | 20 ++++++++++---------- MLPnP/Rodrigues2.m | 1 + MLPnP/optim_MLPnP_GN.m | 10 +++------- MLPnP/residualsAndJacobian.m | 3 ++- MLPnP/skew.m | 2 +- normc.m | 8 ++++++++ 8 files changed, 27 insertions(+), 25 deletions(-) create mode 100644 normc.m diff --git a/MLPnP/MLPNP_with_COV.m b/MLPnP/MLPNP_with_COV.m index 3771381..e64dfc3 100644 --- a/MLPnP/MLPNP_with_COV.m +++ b/MLPnP/MLPNP_with_COV.m @@ -1,5 +1,3 @@ function [R,t] = MLPNP_with_COV(XX,xx,cov) - T = MLPnP(XX, normc(xx),cov); - R = T(1:3,1:3); - t = T(1:3,4); + [R,t] = MLPnP(XX, normc(xx),cov); end diff --git a/MLPnP/MLPNP_without_COV.m b/MLPnP/MLPNP_without_COV.m index 622cd92..25f5892 100644 --- a/MLPnP/MLPNP_without_COV.m +++ b/MLPnP/MLPNP_without_COV.m @@ -1,5 +1,3 @@ function [R,t] = MLPNP_without_COV(XX, xx, cov) - T = MLPnP(XX, normc(xx)); - R = T(1:3,1:3); - t = T(1:3,4); + [R,t] = MLPnP(XX, normc(xx)); end diff --git a/MLPnP/MLPnP.m b/MLPnP/MLPnP.m index 781c645..e0b5625 100644 --- a/MLPnP/MLPnP.m +++ b/MLPnP/MLPnP.m @@ -38,9 +38,10 @@ % here K\ and /K' are the Jacobians of the image to % bearing vector transformation (inverse calibration % matrix K. Details in the paper. -% output: 1. T - 4x4 transformation matrix [R T;0 0 0 1] +% output: 1. R - rotation matrix +% 2. t - translation vector -function [T] = MLPnP(points3D, v, cov) +function [Rout, tout] = MLPnP(points3D, v, cov) use_cov = 1; % if cov is not given don't use it if nargin < 3 @@ -51,7 +52,9 @@ % matrix of null space vectors r and s r = zeros(3,nrPts); s = zeros(3,nrPts); - cov_reduced = zeros(2,2,nrPts); + if use_cov + cov_reduced = zeros(2,2,nrPts); + end % compute null spaces of bearing vector v: null(v') for i=1:nrPts @@ -73,12 +76,11 @@ A = zeros(nrPts,12); if (rank(S) == 2) planar = 1; - points3D1 = eigRot'*(points3D); - points3Dn = [points3D1;ones(1,nrPts)]; + points3Dn = eigRot'*(points3D); % create reduced design matrix A = zeros(nrPts,9); else - points3Dn = [points3D;ones(1,nrPts)]; + points3Dn = points3D; end % stochastic model @@ -175,7 +177,7 @@ P(:,1) = cross(P(:,2),P(:,3)); %SVD to find the best rotation matrix in the Frobenius sense - [U2,~,V2] = svd(P(1:3,1:3)); + [U2,~,V2] = svd(P); R = U2*V2'; if det(R) < 0 R = -1*R; @@ -210,7 +212,7 @@ P = reshape(v1(1:9,end),3,3)'; scalefact = (abs(norm(P(:,1))*norm(P(:,2))*norm(P(:,3))))^(1/3); %SVD to find the best rotation matrix in the Frobenius sense - [U2,~,V2] = svd(P(1:3,1:3)); + [U2,~,V2] = svd(P); R = U2*V2'; %cw if det(R) < 0 R = -1*R; @@ -243,6 +245,4 @@ optimFlags.maxit = 5; optimFlags.tau = 1e-4; [Rout, tout] = optim_MLPnP_GN(T(1:3,1:3), T(1:3,4), points3D, r, s, Kll, optimFlags); - - T = [Rout tout]; end diff --git a/MLPnP/Rodrigues2.m b/MLPnP/Rodrigues2.m index 71d2b9f..51d9879 100644 --- a/MLPnP/Rodrigues2.m +++ b/MLPnP/Rodrigues2.m @@ -16,6 +16,7 @@ % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. % 28.06.2016 Steffen Urban +% 18.08.2023 Shi Shenglei function R2 = Rodrigues2(R1) diff --git a/MLPnP/optim_MLPnP_GN.m b/MLPnP/optim_MLPnP_GN.m index 4e11c24..a9aef08 100644 --- a/MLPnP/optim_MLPnP_GN.m +++ b/MLPnP/optim_MLPnP_GN.m @@ -18,19 +18,15 @@ % 28.06.2016 Steffen Urban % 18.08.2023 Shi Shenglei -function [Rout, tout] = optim_MLPnP_GN(R, t, points3D, rnull, snull, P, optimFlags) - nrPts = size(points3D,2); - +function [Rout, tout] = optim_MLPnP_GN(R, t, points3D, rnull, snull, invKll, optimFlags) % optim params epsParam = optimFlags.epsP; epsFunc = optimFlags.epsF; % iteration params cnt = 0; - stop = false; - invKll = P; - while cnt < optimFlags.maxit && stop == 0 - [r, J] = residualsAndJacobian(R, t, rnull, snull, points3D); + while cnt < optimFlags.maxit + [r, J] = residualsAndJacobian(R, t, points3D, rnull, snull); % design matrix N = J.'*invKll*J; % System matrix diff --git a/MLPnP/residualsAndJacobian.m b/MLPnP/residualsAndJacobian.m index bf24aee..141a1a0 100644 --- a/MLPnP/residualsAndJacobian.m +++ b/MLPnP/residualsAndJacobian.m @@ -17,7 +17,8 @@ % 28.06.2016 Steffen Urban % 18.08.2023 Shi Shenglei -function [err,J] = residualsAndJacobian(R, t, r, s, points3D) + +function [err,J] = residualsAndJacobian(R, t, points3D, r, s) nrPts = size(points3D,2); err = zeros(2*nrPts,1); J = zeros(2*nrPts,6); diff --git a/MLPnP/skew.m b/MLPnP/skew.m index 2eeb23d..6cb9c99 100644 --- a/MLPnP/skew.m +++ b/MLPnP/skew.m @@ -15,7 +15,7 @@ % with this program; if not, write to the Free Software Foundation, Inc., % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -% 28.06.2016 Steffen Urban +% 18.08.2023 Shi Shenglei function [wv] = skew(v) wv = [ 0 -v(3) v(2); diff --git a/normc.m b/normc.m new file mode 100644 index 0000000..62c81c1 --- /dev/null +++ b/normc.m @@ -0,0 +1,8 @@ +function xx = normc(x) + row = size(x, 1); + col = size(x, 2); + xx = zeros(row, col); + for i = 1 : col + xx(:, i) = x(:, i) / norm(x(:, i)); + end +end